1 |
|
|
/** |
2 |
|
|
* @file polynome.hpp |
3 |
|
|
* @author Maximilien Naveau (maximilien.naveau@gmail.com) |
4 |
|
|
* @brief Polynomes object for trajectories. |
5 |
|
|
* @version 0.1 |
6 |
|
|
* @date 2019-11-06 |
7 |
|
|
* |
8 |
|
|
* @copyright Copyright (c) 2019 |
9 |
|
|
* |
10 |
|
|
*/ |
11 |
|
|
#pragma once |
12 |
|
|
|
13 |
|
|
#include <array> |
14 |
|
|
|
15 |
|
|
namespace blmc_robots |
16 |
|
|
{ |
17 |
|
|
|
18 |
|
|
/** |
19 |
|
|
* @brief Simple class that defines \f$ P(x) \f$ a polynome of order ORDER. |
20 |
|
|
* It provide simple methods to compute \f$ P(x) \f$, |
21 |
|
|
* \f$ \frac{dP}{dx}(x) \f$, and \f$ \frac{dP^2}{dx^2}(x) \f$ |
22 |
|
|
* |
23 |
|
|
* @tparam ORDER is the order of the polynome |
24 |
|
|
*/ |
25 |
|
|
template<int ORDER> |
26 |
|
|
class Polynome |
27 |
|
|
{ |
28 |
|
|
/*! Type of the container for the poynome coefficients */ |
29 |
|
|
typedef std::array<double, ORDER> Coefficients; |
30 |
|
|
public: |
31 |
|
|
|
32 |
|
|
/*! Constructor */ |
33 |
|
|
Polynome(); |
34 |
|
|
|
35 |
|
|
/*! Destructor */ |
36 |
|
|
~Polynome(); |
37 |
|
|
|
38 |
|
|
/*! Compute the value. */ |
39 |
|
|
double compute(double x); |
40 |
|
|
|
41 |
|
|
/*! Compute the value of the derivative. */ |
42 |
|
|
double compute_derivative(double x); |
43 |
|
|
|
44 |
|
|
/*! Compute the value of the second derivative. */ |
45 |
|
|
double compute_sec_derivative(double x); |
46 |
|
|
|
47 |
|
|
/*! Get the coefficients. */ |
48 |
|
|
void get_coefficients(Coefficients &coefficients) const; |
49 |
|
|
|
50 |
|
|
/*! Set the coefficients. */ |
51 |
|
|
void set_coefficients(const Coefficients &coefficients); |
52 |
|
|
|
53 |
|
|
inline int degree() |
54 |
|
|
{ return ORDER; }; |
55 |
|
|
|
56 |
|
|
/*! Print the coefficient. */ |
57 |
|
|
void print() const; |
58 |
|
|
|
59 |
|
|
protected: |
60 |
|
|
|
61 |
|
|
/*! Vector of coefficients. */ |
62 |
|
|
std::array<double, ORDER+1> coefficients_; |
63 |
|
|
}; |
64 |
|
|
|
65 |
|
|
/** |
66 |
|
|
* @brief Simple class that defines \f$ P(t) \f$ a polynome of order ORDER. |
67 |
|
|
* With \f$ t \f$ being the time in any units. |
68 |
|
|
* It provide simple methods to compute safely \f$ P(time) \f$, |
69 |
|
|
* \f$ \frac{dP}{dt}(t) \f$, and \f$ \frac{dP^2}{dt^2}(t) \f$ |
70 |
|
|
* |
71 |
|
|
* @tparam ORDER |
72 |
|
|
*/ |
73 |
|
|
template<int ORDER> |
74 |
|
|
class TimePolynome: public Polynome<ORDER> |
75 |
|
|
{ |
76 |
|
|
public: |
77 |
|
|
|
78 |
|
|
/*! Constructor */ |
79 |
|
2 |
TimePolynome() |
80 |
|
2 |
{ |
81 |
|
2 |
final_time_ = 0.0; |
82 |
|
2 |
init_pose_ = 0.0; |
83 |
|
2 |
init_speed_ = 0.0; |
84 |
|
2 |
init_acc_ = 0.0; |
85 |
|
2 |
final_pose_ = 0.0; |
86 |
|
2 |
final_speed_ = 0.0; |
87 |
|
2 |
final_acc_ = 0.0; |
88 |
|
2 |
}; |
89 |
|
|
|
90 |
|
|
/*! Destructor */ |
91 |
|
2 |
~TimePolynome(){}; |
92 |
|
|
|
93 |
|
|
/*! Compute the value. */ |
94 |
|
|
double compute(double t); |
95 |
|
|
|
96 |
|
|
/*! Compute the value of the derivative. */ |
97 |
|
|
double compute_derivative(double t); |
98 |
|
|
|
99 |
|
|
/*! Compute the value of the second derivative. */ |
100 |
|
|
double compute_sec_derivative(double t); |
101 |
|
|
|
102 |
|
2 |
double get_final_time() const |
103 |
|
2 |
{return final_time_;} |
104 |
|
2 |
double get_init_pose() const |
105 |
|
2 |
{return init_pose_;} |
106 |
|
2 |
double get_init_speed() const |
107 |
|
2 |
{return init_speed_;} |
108 |
|
2 |
double get_init_acc() const |
109 |
|
2 |
{return init_acc_;} |
110 |
|
2 |
double get_final_pose() const |
111 |
|
2 |
{return final_pose_;} |
112 |
|
2 |
double get_final_speed() const |
113 |
|
2 |
{return final_speed_;} |
114 |
|
2 |
double get_final_acc() const |
115 |
|
2 |
{return final_acc_;} |
116 |
|
|
|
117 |
|
|
/** |
118 |
|
|
* @brief Computes a polynome trajectory according to the following |
119 |
|
|
* constraints: |
120 |
|
|
* \f{eqnarray*}{ |
121 |
|
|
* P(0) &=& init_pose \\ |
122 |
|
|
* P(0) &=& init_speed = 0 \\ |
123 |
|
|
* P(0) &=& init_acc = 0 \\ |
124 |
|
|
* P(final_time_) &=& final_pose \\ |
125 |
|
|
* P(final_time_) &=& final_speed = 0 \\ |
126 |
|
|
* P(final_time_) &=& final_acc = 0 |
127 |
|
|
* \f} |
128 |
|
|
* |
129 |
|
|
* @param final_time is used in the constraints. |
130 |
|
|
* @param init_pose is used in the constraints. |
131 |
|
|
* @param init_speed is used in the constraints. |
132 |
|
|
* @param final_pose is used in the constraints. |
133 |
|
|
*/ |
134 |
|
|
void set_parameters( |
135 |
|
|
double final_time, |
136 |
|
|
double init_pose, |
137 |
|
|
double init_speed, |
138 |
|
|
double final_pose); |
139 |
|
|
|
140 |
|
|
protected: |
141 |
|
|
double final_time_; /**< store the inputs for later access */ |
142 |
|
|
double init_pose_; /**< store the inputs for later access */ |
143 |
|
|
double init_speed_; /**< store the inputs for later access */ |
144 |
|
|
double init_acc_; /**< store the inputs for later access */ |
145 |
|
|
double final_pose_; /**< store the inputs for later access */ |
146 |
|
|
double final_speed_; /**< store the inputs for later access */ |
147 |
|
|
double final_acc_; /**< store the inputs for later access */ |
148 |
|
|
}; |
149 |
|
|
|
150 |
|
|
}// namespace blmc_robots |
151 |
|
|
|
152 |
|
|
#include "blmc_robots/mathematics/polynome.hxx" |