hpp-core 4.15.1
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>
36#include <hpp/pinocchio/device.hh>
37#include <hpp/pinocchio/liegroup-element.hh>
38
39namespace hpp {
40namespace core {
41namespace path {
44
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
87template <int _PolynomeBasis, int _Order>
88class HPP_CORE_DLLAPI Spline : public Path {
89 public:
90 enum {
91 PolynomeBasis = _PolynomeBasis,
92 Order = _Order,
93 NbCoeffs = _Order + 1,
94 NbPowerOfT = 2 * NbCoeffs + 1
95 };
96
97 typedef internal::sbf_traits<PolynomeBasis, Order> sbf_traits;
98 typedef internal::spline_basis_function<PolynomeBasis, Order> BasisFunction_t;
99 typedef Eigen::Matrix<value_type, NbPowerOfT, 1> PowersOfT_t;
100 typedef typename sbf_traits::Coeffs_t BasisFunctionVector_t;
101 typedef typename sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t;
102
103 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
105 typedef Eigen::Map<const vector_t, Eigen::Aligned> ConstParameterVector_t;
106 typedef Eigen::Map<vector_t, Eigen::Aligned> ParameterVector_t;
107
108 typedef shared_ptr<Spline> Ptr_t;
109 typedef weak_ptr<Spline> WkPtr_t;
110
111 size_type parameterSize() const { return parameterSize_; }
112
121 const value_type& t) const {
122 assert(res.size() == NbCoeffs);
123 impl_paramDerivative(res, t);
124 }
125
128 assert(dParam.size() == NbCoeffs * parameterSize_);
129 impl_paramIntegrate(dParam);
130 }
131
136
141 vectorOut_t res) const;
142
149 const value_type& u,
151
153 const value_type& u,
154 vectorOut_t res) {
155 assert(res.size() == NbCoeffs);
157 timeFreeBasisFunctionDerivative(order, u, tmp);
158 res = tmp;
159 }
160
166 void basisFunctionDerivative(const size_type order, const value_type& u,
167 BasisFunctionVector_t& res) const;
168
169 void basisFunctionDerivative(const size_type order, const value_type& u,
170 vectorOut_t res) const {
171 assert(res.size() == NbCoeffs);
173 basisFunctionDerivative(order, u, tmp);
174 res = tmp;
175 }
176
179 void maxVelocity(vectorOut_t res) const;
180
187 const size_type order, BasisFunctionIntegralMatrix_t& res) const;
188
190 matrixOut_t res) const {
191 // assert (res.size() == NbCoeffs);
193 squaredNormBasisFunctionIntegral(order, tmp);
194 res = tmp;
195 }
196
197 virtual Configuration_t initial() const {
198 Configuration_t q(outputSize());
199 bool res = operator()(q, timeRange().first);
200 assert(res);
201 (void)res;
202 return q;
203 }
204
205 virtual Configuration_t end() const {
206 Configuration_t q(outputSize());
207 bool res = operator()(q, timeRange().second);
208 assert(res);
209 (void)res;
210 return q;
211 }
212
216 const Configuration_t& base() const { return base_.vector(); }
217
219 void base(const Configuration_t& q) { base_.vector() = q; }
220
222 const ParameterMatrix_t& parameters() const { return parameters_; }
223
226 void parameters(const ParameterMatrix_t& m) { parameters_ = m; }
227
230 return ConstParameterVector_t(parameters_.data(), parameters_.size());
231 }
232
235 ParameterVector_t(parameters_.data(), parameters_.size()) = p;
236 }
237
238 PathPtr_t copy() const {
239 Ptr_t other(new Spline(*this));
240 other->init(other);
241 return other;
242 }
243
244 PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
245 Ptr_t other(new Spline(*this, constraints));
246 other->init(other);
247 return other;
248 }
249
250 virtual ~Spline() {}
251
252 static Ptr_t create(const DevicePtr_t& robot, const interval_t& interval,
253 const ConstraintSetPtr_t& constraints) {
254 Ptr_t shPtr(new Spline(robot, interval, constraints));
255 shPtr->init(shPtr);
256 return shPtr;
257 }
258
266 static void value(pinocchio::LiegroupElementConstRef base,
267 Eigen::Ref<const ParameterMatrix_t> params,
268 const value_type& u, ConfigurationOut_t config,
269 vectorOut_t velocity);
270
271 protected:
272 Spline(const DevicePtr_t& robot, const interval_t& interval,
273 const ConstraintSetPtr_t& constraints)
274 : Path(interval, robot->configSize(), robot->numberDof(), constraints),
275 parameterSize_(robot->numberDof()),
276 robot_(robot),
277 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
278 parameters_((int)NbCoeffs, parameterSize_),
279 velocity_(parameterSize_) {
280 powersOfT_(0) = 1;
281 for (size_type i = 1; i < NbPowerOfT; ++i)
282 powersOfT_(i) = powersOfT_(i - 1) * length();
283 }
284
285 Spline(const Spline& path);
286
287 Spline(const Spline& path, const ConstraintSetPtr_t& constraints);
288
289 void init(const Ptr_t& self) {
290 Path::init(self);
291 weak_ = self;
292 }
293
294 std::ostream& print(std::ostream& os) const;
295
296 bool impl_compute(ConfigurationOut_t configuration, value_type t) const;
297
299 size_type order) const;
300
301 void impl_paramDerivative(vectorOut_t res, const value_type& t) const;
302
304
306 const value_type& t1) const;
307
319
320 private:
321 WkPtr_t weak_;
322
323 mutable vector_t velocity_;
324 mutable PowersOfT_t powersOfT_;
325
326 friend class steeringMethod::Spline<_PolynomeBasis, _Order>;
327
328 private:
329 Spline() {}
330 HPP_SERIALIZABLE();
331}; // class Spline
333} // namespace path
334} // namespace core
335} // namespace hpp
336#endif // HPP_CORE_PATH_SPLINE_HH
Definition: path.hh:71
Definition: spline.hh:88
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor > ParameterMatrix_t
Definition: spline.hh:104
void base(const Configuration_t &q)
Definition: spline.hh:219
const Configuration_t & base() const
Definition: spline.hh:216
void parameterDerivativeCoefficients(vectorOut_t res, const value_type &t) const
Definition: spline.hh:120
std::ostream & print(std::ostream &os) const
void parameterIntegrate(vectorIn_t dParam)
Adds dParam to the parameters.
Definition: spline.hh:127
virtual Configuration_t initial() const
Get the initial configuration.
Definition: spline.hh:197
Eigen::Matrix< value_type, NbPowerOfT, 1 > PowersOfT_t
Definition: spline.hh:99
Spline(const Spline &path)
static Ptr_t create(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:252
weak_ptr< Spline > WkPtr_t
Definition: spline.hh:109
virtual ~Spline()
Definition: spline.hh:250
void impl_velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
ParameterMatrix_t parameters_
Definition: spline.hh:318
void impl_paramIntegrate(vectorIn_t dParam)
sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t
Definition: spline.hh:101
virtual Configuration_t end() const
Get the final configuration.
Definition: spline.hh:205
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res)
Definition: spline.hh:152
shared_ptr< Spline > Ptr_t
Definition: spline.hh:108
Eigen::Map< const vector_t, Eigen::Aligned > ConstParameterVector_t
Definition: spline.hh:105
Eigen::Map< vector_t, Eigen::Aligned > ParameterVector_t
Definition: spline.hh:106
void parameters(const ParameterMatrix_t &m)
Definition: spline.hh:226
void squaredNormIntegralDerivative(const size_type order, vectorOut_t res) const
void rowParameters(vectorIn_t p)
Set the parameters.
Definition: spline.hh:234
void basisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res) const
Definition: spline.hh:169
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:189
void maxVelocity(vectorOut_t res) const
sbf_traits::Coeffs_t BasisFunctionVector_t
Definition: spline.hh:100
bool impl_compute(ConfigurationOut_t configuration, value_type t) const
Function evaluation without applying constraints.
LiegroupElement base_
Definition: spline.hh:314
PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: spline.hh:244
DevicePtr_t robot_
Definition: spline.hh:310
value_type squaredNormIntegral(const size_type order) const
Spline(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:272
internal::spline_basis_function< PolynomeBasis, Order > BasisFunction_t
Definition: spline.hh:98
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:222
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:229
PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: spline.hh:238
size_type parameterSize() const
Definition: spline.hh:111
void init(const Ptr_t &self)
Definition: spline.hh:289
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:97
size_type parameterSize_
Robot number of degrees of freedom.
Definition: spline.hh:309
Definition: spline.hh:48
#define HPP_CORE_DLLAPI
Definition: config.hh:64
PolynomeBasisType
Definition: spline.hh:45
@ BernsteinBasis
Definition: spline.hh:45
@ CanonicalPolynomeBasis
Definition: spline.hh:45
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:108
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:106
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