hpp-core  4.13.0
Implement basic classes for canonical path planning for kinematic chains.
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 
39 namespace hpp {
40 namespace core {
41 namespace path {
44 
46 
48 namespace internal {
49 template <int SplineType, int Degree>
50 struct spline_basis_function;
51 template <int SplineType, int Degree>
52 struct 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 
87 template <int _PolynomeBasis, int _Order>
88 class 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 
135  value_type squaredNormIntegral(const size_type order) const;
136 
140  void squaredNormIntegralDerivative(const size_type order,
141  vectorOut_t res) const;
142 
148  static void timeFreeBasisFunctionDerivative(const size_type order,
149  const value_type& u,
150  BasisFunctionVector_t& res);
151 
153  const value_type& u,
154  vectorOut_t res) {
155  assert(res.size() == NbCoeffs);
156  BasisFunctionVector_t tmp;
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);
172  BasisFunctionVector_t tmp;
173  basisFunctionDerivative(order, u, tmp);
174  res = tmp;
175  }
176 
179  void maxVelocity(vectorOut_t res) const;
180 
186  void squaredNormBasisFunctionIntegral(
187  const size_type order, BasisFunctionIntegralMatrix_t& res) const;
188 
190  matrixOut_t res) const {
191  // assert (res.size() == NbCoeffs);
192  BasisFunctionIntegralMatrix_t tmp;
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 
229  ConstParameterVector_t rowParameters() const {
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 
298  void impl_derivative(vectorOut_t res, const value_type& t,
299  size_type order) const;
300 
301  void impl_paramDerivative(vectorOut_t res, const value_type& t) const;
302 
303  void impl_paramIntegrate(vectorIn_t dParam);
304 
305  void impl_velocityBound(vectorOut_t result, const value_type& t0,
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
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:123
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:164
virtual Configuration_t initial() const
Get the initial configuration.
Definition: spline.hh:197
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:107
pinocchio::size_type size_type
Definition: fwd.hh:162
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:157
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
Definition: spline.hh:45
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:210
void init(const PathWkPtr_t &self)
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:159
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:119
pinocchio::vector_t vector_t
Definition: fwd.hh:209
LiegroupElement base_
Definition: spline.hh:314
pinocchio::value_type value_type
Definition: fwd.hh:163
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:156
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:211
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
Definition: path.hh:71
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:105
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:176
void parameters(const ParameterMatrix_t &m)
Definition: spline.hh:226