integral_cost.h 1.66 KB
Newer Older
1
/**
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
2
3
4
5
6
7
 * \file bezier_curve.h
 * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
 * \author Steve T.
 * \version 0.1
 * \date 06/17/2013
 */
8
9
10
11

#ifndef _CLASS_QUADRATIC_COST
#define _CLASS_QUADRATIC_COST

Steve T's avatar
Steve T committed
12
13
#include "ndcurves/optimization/definitions.h"
#include "ndcurves/optimization/details.h"
14
15
16

#include <Eigen/Core>

Steve T's avatar
Steve T committed
17
namespace ndcurves {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
18
namespace optimization {
19

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
20
21
22
23
24
25
26
enum integral_cost_flag {
  DISTANCE = 0x000,
  VELOCITY = 0x001,
  ACCELERATION = 0x002,
  JERK = 0x003,
  FOURTH = 0x004,
  FIFTH = 0x005
27
28
};

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
29
30
31
32
33
34
template <typename Point, typename Numeric>
quadratic_variable<Numeric> compute_integral_cost_internal(const problem_data<Point, Numeric>& pData,
                                                           const std::size_t num_derivate) {
  typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
  typedef typename bezier_t::t_point_t t_point_t;
  typedef typename t_point_t::const_iterator cit_point_t;
35
36
  bezier_t acc = pData.bezier->compute_derivate(num_derivate);
  const t_point_t& wps = acc.waypoints();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
37
38
  quadratic_variable<Numeric> res(
      bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
39
  return res;
40
41
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
42
43
44
45
46
template <typename Point, typename Numeric>
quadratic_variable<Numeric> compute_integral_cost(const problem_data<Point, Numeric>& pData,
                                                  const integral_cost_flag flag) {
  std::size_t size = (std::size_t)(flag);
  return compute_integral_cost_internal<Point, Numeric>(pData, size);
47
48
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
49
}  // namespace optimization
Steve T's avatar
Steve T committed
50
}  // namespace ndcurves
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
51
#endif  //_CLASS_QUADRATIC_COST