hpp-core: include/hpp/core/path/spline.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
spline.hh
Go to the documentation of this file.
1// Copyright (c) 2017 CNRS
2// Authors: Joseph Mirabel
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CORE_PATH_SPLINE_HH
30#define HPP_CORE_PATH_SPLINE_HH
31
32#include <hpp/core/config.hh>
33#include <hpp/core/fwd.hh>
34#include <hpp/core/path.hh>
35#include <hpp/core/steering-method/fwd.hh>
36#include <hpp/pinocchio/device.hh>
37#include <hpp/pinocchio/liegroup-element.hh>
38
39namespace hpp {
40namespace core {
41namespace path {
44
45 enum PolynomeBasisType { CanonicalPolynomeBasis, BernsteinBasis };
46
48namespace internal {
49template <int SplineType, int Degree>
50struct spline_basis_function;
51template <int SplineType, int Degree>
52struct sbf_traits {
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;
56};
57} // namespace internal
59
90template <int _PolynomeBasis, int _Order>
91 class HPP_CORE_DLLAPI Spline : public Path {
92 public:
93 enum {
94 PolynomeBasis = _PolynomeBasis,
95 Order = _Order,
96 NbCoeffs = _Order + 1,
97 NbPowerOfT = 2 * NbCoeffs + 1
98 };
99
100 typedef internal::sbf_traits<PolynomeBasis, Order> sbf_traits;
101 typedef internal::spline_basis_function<PolynomeBasis, Order> BasisFunction_t;
102 typedef Eigen::Matrix<value_type, NbPowerOfT, 1> PowersOfT_t;
103 typedef typename sbf_traits::Coeffs_t BasisFunctionVector_t;
104 typedef typename sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t;
105
106 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
107 ParameterMatrix_t;
108 typedef Eigen::Map<const vector_t, Eigen::Aligned> ConstParameterVector_t;
109 typedef Eigen::Map<vector_t, Eigen::Aligned> ParameterVector_t;
110
111 typedef shared_ptr<Spline> Ptr_t;
112 typedef weak_ptr<Spline> WkPtr_t;
113
114 size_type parameterSize() const { return parameterSize_; }
115
123 void parameterDerivativeCoefficients(vectorOut_t res,
124 const value_type& t) const {
125 assert(res.size() == NbCoeffs);
126 impl_paramDerivative(res, t);
127 }
128
130 void parameterIntegrate(vectorIn_t dParam) {
131 assert(dParam.size() == NbCoeffs * parameterSize_);
132 impl_paramIntegrate(dParam);
133 }
134
138 value_type squaredNormIntegral(const size_type order) const;
139
143 void squaredNormIntegralDerivative(const size_type order,
144 vectorOut_t res) const;
145
151 static void timeFreeBasisFunctionDerivative(const size_type order,
152 const value_type& u,
153 BasisFunctionVector_t& res);
154
155 static void timeFreeBasisFunctionDerivative(const size_type order,
156 const value_type& u,
157 vectorOut_t res) {
158 assert(res.size() == NbCoeffs);
159 BasisFunctionVector_t tmp;
160 timeFreeBasisFunctionDerivative(order, u, tmp);
161 res = tmp;
162 }
163
169 void basisFunctionDerivative(const size_type order, const value_type& u,
170 BasisFunctionVector_t& res) const;
171
172 void basisFunctionDerivative(const size_type order, const value_type& u,
173 vectorOut_t res) const {
174 assert(res.size() == NbCoeffs);
175 BasisFunctionVector_t tmp;
176 basisFunctionDerivative(order, u, tmp);
177 res = tmp;
178 }
179
182 void maxVelocity(vectorOut_t res) const;
183
189 void squaredNormBasisFunctionIntegral(
190 const size_type order, BasisFunctionIntegralMatrix_t& res) const;
191
192 void squaredNormBasisFunctionIntegral(const size_type order,
193 matrixOut_t res) const {
194 // assert (res.size() == NbCoeffs);
195 BasisFunctionIntegralMatrix_t tmp;
196 squaredNormBasisFunctionIntegral(order, tmp);
197 res = tmp;
198 }
199
200 virtual Configuration_t initial() const {
201 Configuration_t q(outputSize());
202 bool res = eval(q, timeRange().first);
203 assert(res);
204 (void)res;
205 return q;
206 }
207
208 virtual Configuration_t end() const {
209 Configuration_t q(outputSize());
210 bool res = eval(q, timeRange().second);
211 assert(res);
212 (void)res;
213 return q;
214 }
215
219 const Configuration_t& base() const { return base_.vector(); }
220
222 void base(const Configuration_t& q) { base_.vector() = q; }
223
225 const ParameterMatrix_t& parameters() const { return parameters_; }
226
229 void parameters(const ParameterMatrix_t& m) { parameters_ = m; }
230
232 ConstParameterVector_t rowParameters() const {
233 return ConstParameterVector_t(parameters_.data(), parameters_.size());
234 }
235
237 void rowParameters(vectorIn_t p) {
238 ParameterVector_t(parameters_.data(), parameters_.size()) = p;
239 }
240
241 PathPtr_t copy() const {
242 Ptr_t other(new Spline(*this));
243 other->init(other);
244 return other;
245 }
246
247 PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
248 Ptr_t other(new Spline(*this, constraints));
249 other->init(other);
250 return other;
251 }
252
253 virtual ~Spline() {}
254
265 static Ptr_t create(const DevicePtr_t& robot, const interval_t& interval,
266 const ConstraintSetPtr_t& constraints,
267 bool se3Output = false) {
268 Ptr_t shPtr(new Spline(robot, interval, constraints, se3Output));
269 shPtr->init(shPtr);
270 return shPtr;
271 }
272
280 static void value(pinocchio::LiegroupElementConstRef base,
281 Eigen::Ref<const ParameterMatrix_t> params,
282 const value_type& u, ConfigurationOut_t config,
283 vectorOut_t velocity);
284
285 protected:
295 Spline(const DevicePtr_t& robot, const interval_t& interval,
296 const ConstraintSetPtr_t& constraints, bool se3Output = false)
297 : Path(interval, robot->configSize(), robot->numberDof(), constraints),
298 parameterSize_(robot->numberDof()),
299 robot_(robot),
300 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
301 parameters_((int)NbCoeffs, parameterSize_),
302 velocity_(parameterSize_) {
303 powersOfT_(0) = 1;
304 for (size_type i = 1; i < NbPowerOfT; ++i)
305 powersOfT_(i) = powersOfT_(i - 1) * length();
306 if (se3Output) base_ = robot->configSpace()->vectorSpacesMerged();
307 }
308
309 Spline(const Spline& path);
310
311 Spline(const Spline& path, const ConstraintSetPtr_t& constraints);
312
313 void init(const Ptr_t& self) {
314 Path::init(self);
315 weak_ = self;
316 }
317
318 std::ostream& print(std::ostream& os) const;
319
320 bool impl_compute(ConfigurationOut_t configuration, value_type t) const;
321
327 void impl_derivative(vectorOut_t res, const value_type& t,
328 size_type order) const;
329
330 void impl_paramDerivative(vectorOut_t res, const value_type& t) const;
331
332 void impl_paramIntegrate(vectorIn_t dParam);
333
334 void impl_velocityBound(vectorOut_t result, const value_type& t0,
335 const value_type& t1) const;
336
338 size_type parameterSize_;
339 DevicePtr_t robot_;
343 LiegroupElement base_;
347 ParameterMatrix_t parameters_;
348
349 private:
350 WkPtr_t weak_;
351
352 mutable vector_t velocity_;
353 mutable PowersOfT_t powersOfT_;
354
355 friend class steeringMethod::Spline<_PolynomeBasis, _Order>;
356
357 private:
358 Spline() {}
359 HPP_SERIALIZABLE();
360}; // class Spline
362} // namespace path
363} // namespace core
364} // namespace hpp
365#endif // HPP_CORE_PATH_SPLINE_HH
hpp::core::Path
Definition path.hh:71
hpp::core::path::Spline
Definition spline.hh:91
hpp::core::path::Spline::ParameterMatrix_t
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor > ParameterMatrix_t
Definition spline.hh:107
hpp::core::path::Spline::base
void base(const Configuration_t &q)
Definition spline.hh:222
hpp::core::path::Spline::Spline
Spline(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints, bool se3Output=false)
Definition spline.hh:295
hpp::core::path::Spline::base
const Configuration_t & base() const
Definition spline.hh:219
hpp::core::path::Spline::parameterDerivativeCoefficients
void parameterDerivativeCoefficients(vectorOut_t res, const value_type &t) const
Definition spline.hh:123
hpp::core::path::Spline::print
std::ostream & print(std::ostream &os) const
hpp::core::path::Spline::parameterIntegrate
void parameterIntegrate(vectorIn_t dParam)
Adds dParam to the parameters.
Definition spline.hh:130
hpp::core::path::Spline::initial
virtual Configuration_t initial() const
Get the initial configuration.
Definition spline.hh:200
hpp::core::path::Spline::PowersOfT_t
Eigen::Matrix< value_type, NbPowerOfT, 1 > PowersOfT_t
Definition spline.hh:102
hpp::core::path::Spline::create
static Ptr_t create(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints, bool se3Output=false)
Definition spline.hh:265
hpp::core::path::Spline::Spline
Spline(const Spline &path)
hpp::core::path::Spline::WkPtr_t
weak_ptr< Spline > WkPtr_t
Definition spline.hh:112
hpp::core::path::Spline::~Spline
virtual ~Spline()
Definition spline.hh:253
hpp::core::path::Spline::impl_velocityBound
void impl_velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
hpp::core::path::Spline::parameters_
ParameterMatrix_t parameters_
Definition spline.hh:347
hpp::core::path::Spline::impl_paramIntegrate
void impl_paramIntegrate(vectorIn_t dParam)
hpp::core::path::Spline::BasisFunctionIntegralMatrix_t
sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t
Definition spline.hh:104
hpp::core::path::Spline::end
virtual Configuration_t end() const
Get the final configuration.
Definition spline.hh:208
hpp::core::path::Spline::timeFreeBasisFunctionDerivative
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res)
Definition spline.hh:155
hpp::core::path::Spline::Ptr_t
shared_ptr< Spline > Ptr_t
Definition spline.hh:111
hpp::core::path::Spline::ConstParameterVector_t
Eigen::Map< const vector_t, Eigen::Aligned > ConstParameterVector_t
Definition spline.hh:108
hpp::core::path::Spline::ParameterVector_t
Eigen::Map< vector_t, Eigen::Aligned > ParameterVector_t
Definition spline.hh:109
hpp::core::path::Spline::parameters
void parameters(const ParameterMatrix_t &m)
Definition spline.hh:229
hpp::core::path::Spline::squaredNormIntegralDerivative
void squaredNormIntegralDerivative(const size_type order, vectorOut_t res) const
hpp::core::path::Spline::rowParameters
void rowParameters(vectorIn_t p)
Set the parameters.
Definition spline.hh:237
hpp::core::path::Spline::basisFunctionDerivative
void basisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res) const
Definition spline.hh:172
hpp::core::path::Spline::impl_derivative
void impl_derivative(vectorOut_t res, const value_type &t, size_type order) const
hpp::core::path::Spline::squaredNormBasisFunctionIntegral
void squaredNormBasisFunctionIntegral(const size_type order, matrixOut_t res) const
Definition spline.hh:192
hpp::core::path::Spline::maxVelocity
void maxVelocity(vectorOut_t res) const
hpp::core::path::Spline::BasisFunctionVector_t
sbf_traits::Coeffs_t BasisFunctionVector_t
Definition spline.hh:103
hpp::core::path::Spline::impl_compute
bool impl_compute(ConfigurationOut_t configuration, value_type t) const
Function evaluation without applying constraints.
hpp::core::path::Spline::base_
LiegroupElement base_
Definition spline.hh:343
hpp::core::path::Spline::copy
PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition spline.hh:247
hpp::core::path::Spline::robot_
DevicePtr_t robot_
Definition spline.hh:339
hpp::core::path::Spline::squaredNormIntegral
value_type squaredNormIntegral(const size_type order) const
hpp::core::path::Spline::BasisFunction_t
internal::spline_basis_function< PolynomeBasis, Order > BasisFunction_t
Definition spline.hh:101
hpp::core::path::Spline::value
static void value(pinocchio::LiegroupElementConstRef base, Eigen::Ref< const ParameterMatrix_t > params, const value_type &u, ConfigurationOut_t config, vectorOut_t velocity)
hpp::core::path::Spline::parameters
const ParameterMatrix_t & parameters() const
Each row corresponds to a velocity of the robot.
Definition spline.hh:225
hpp::core::path::Spline::impl_paramDerivative
void impl_paramDerivative(vectorOut_t res, const value_type &t) const
hpp::core::path::Spline::timeFreeBasisFunctionDerivative
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, BasisFunctionVector_t &res)
hpp::core::path::Spline::rowParameters
ConstParameterVector_t rowParameters() const
Concatenate the parameters as one vector (P_0^T, ..., P_n^T).
Definition spline.hh:232
hpp::core::path::Spline::copy
PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition spline.hh:241
hpp::core::path::Spline::parameterSize
size_type parameterSize() const
Definition spline.hh:114
hpp::core::path::Spline::init
void init(const Ptr_t &self)
Definition spline.hh:313
hpp::core::path::Spline::basisFunctionDerivative
void basisFunctionDerivative(const size_type order, const value_type &u, BasisFunctionVector_t &res) const
hpp::core::path::Spline::squaredNormBasisFunctionIntegral
void squaredNormBasisFunctionIntegral(const size_type order, BasisFunctionIntegralMatrix_t &res) const
hpp::core::path::Spline::Spline
Spline(const Spline &path, const ConstraintSetPtr_t &constraints)
hpp::core::path::Spline::sbf_traits
internal::sbf_traits< PolynomeBasis, Order > sbf_traits
Definition spline.hh:100
hpp::core::path::Spline::parameterSize_
size_type parameterSize_
Robot number of degrees of freedom.
Definition spline.hh:338
hpp::core::steeringMethod::Spline
Definition spline.hh:48
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementConstBase
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
device.hh
fwd.hh
hpp::core::path::PolynomeBasisType
PolynomeBasisType
Definition spline.hh:45
hpp::core::path::BernsteinBasis
@ BernsteinBasis
Definition spline.hh:45
hpp::core::path::CanonicalPolynomeBasis
@ CanonicalPolynomeBasis
Definition spline.hh:45
liegroup-element.hh
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::matrixOut_t
constraints::matrixOut_t matrixOut_t
Definition fwd.hh:167
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
path.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()
fwd.hh