1 |
|
|
/** |
2 |
|
|
* @file polynome.cpp |
3 |
|
|
* @author Maximilien Naveau (maximilien.naveau@gmail.com) |
4 |
|
|
* @brief Polynomes object for trajectories. |
5 |
|
|
* |
6 |
|
|
* @version 0.1 |
7 |
|
|
* @date 2019-11-07 |
8 |
|
|
* |
9 |
|
|
* @copyright Copyright (c) 2019 |
10 |
|
|
* |
11 |
|
|
* See https://github.com/jrl-umi3218/jrl-walkgen/blob/master/src/Mathematics/PolynomeFoot.cpp |
12 |
|
|
* for further enhancement. |
13 |
|
|
*/ |
14 |
|
|
|
15 |
|
|
#include <iostream> |
16 |
|
|
#include <blmc_robots/mathematics/polynome.hpp> |
17 |
|
|
|
18 |
|
|
namespace blmc_robots{ |
19 |
|
|
|
20 |
|
|
template <> |
21 |
|
1 |
void TimePolynome<5>::set_parameters( |
22 |
|
|
double final_time, |
23 |
|
|
double init_pose, |
24 |
|
|
double init_speed, |
25 |
|
|
double final_pose) |
26 |
|
|
{ |
27 |
|
|
// save the parameter in the class and do some renaming |
28 |
|
1 |
final_time_ = final_time; |
29 |
|
1 |
init_pose_ = init_pose; |
30 |
|
1 |
init_speed_ = init_speed; |
31 |
|
1 |
init_acc_ = 0.0; |
32 |
|
1 |
final_pose_ = final_pose; |
33 |
|
1 |
final_speed_ = 0.0; |
34 |
|
1 |
final_acc_ = 0.0; |
35 |
|
|
// shortcuts |
36 |
|
1 |
double ft = final_time_; |
37 |
|
1 |
double ip = init_pose_; |
38 |
|
1 |
double is = init_speed; |
39 |
|
1 |
double ia = final_acc_; |
40 |
|
1 |
double fp = final_pose_; |
41 |
|
|
// do the computation using the analytical solution |
42 |
|
|
double tmp; |
43 |
|
1 |
coefficients_[0] = ip; |
44 |
|
1 |
coefficients_[1] = is; |
45 |
|
1 |
coefficients_[2] = ia/2.0; |
46 |
|
|
|
47 |
|
1 |
tmp = ft*ft*ft; |
48 |
|
1 |
coefficients_[3] = (-3.0/2.0*ia*ft*ft-6.0*is*ft - 10.0*ip + 10.0*fp)/tmp; |
49 |
|
1 |
tmp=tmp*ft; |
50 |
|
1 |
coefficients_[4] = ( 3.0/2.0*ia*ft*ft + 8.0*is*ft + 15.0*ip - 15.0*fp)/tmp; |
51 |
|
1 |
tmp=tmp*ft; |
52 |
|
1 |
coefficients_[5] = ( -1.0/2.0*ia*ft*ft - 3.0*is*ft - 6.0*ip + 6.0*fp)/tmp; |
53 |
|
1 |
} |
54 |
|
|
|
55 |
✓✗✓✗
|
3 |
} |