rafaelrojasmiliani / gsplines_cpp

Generalized Splines for Motion Optimization in C++ and python3
MIT License
24 stars 0 forks source link

Function that returns GL spline evaluated ad GL points #49

Open rafaelrojasmiliani opened 2 years ago

rafaelrojasmiliani commented 2 years ago
+-----------------+
| x_0(t_0)        |
| x_0(t_1)        |
| x_0(t_2)        |
|   ....          |
| x_0(t_{nc-1})   |
| x_1(t_0)        |
| x_1(t_1)        |
| x_1(t_2)        |
|   ....          |
| x_1(t_{nc-1})   |
|   ....          |
| x_n(t_0)        |
| x_n(t_1)        |
| x_n(t_2)        |
|   ....          |
| x_n(t_{nc-1})   |
| End of interval |
| x_0(t_0)        |
| x_0(t_1)        |
| x_0(t_2)        |
|   ....          |
| x_0(t_{nc-1})   |
| x_1(t_0)        |
| x_1(t_1)        |
| x_1(t_2)        |
|   ....          |
| x_1(t_{nc-1})   |
|   ....          |
| x_n(t_0)        |
| x_n(t_1)        |
| x_n(t_2)        |
|   ....          |
| x_n(t_{nc-1})   |

We desires

| x_0(t_0)        | x_1(t_0)        | ...        | x_n(t_0)        |
| x_0(t_1)        | x_1(t_1)        | ...        | x_n(t_1)        |
|  ...            ...      ....          ....                      |
| x_0(t_{nc-1})   | x_1(t_{nc-1})   | ...        | x_n(t_{nc-1})   |
|  ...            ...end of interval                        |
| x_0(t_0)        | x_1(t_0)        | ...        | x_n(t_0)        |
| x_0(t_1)        | x_1(t_1)        | ...        | x_n(t_1)        |
|  ...            ...      ....          ....                      |
| x_0(t_{nc-1})   | x_1(t_{nc-1})   | ...        | x_n(t_{nc-1})   |
rafaelrojasmiliani commented 2 years ago

We can see that at each interval, the coefficients constitute the desired matrix in a col-major ordering

rafaelrojasmiliani commented 2 years ago

We can also implement a single function that returns the "value at" that returns a map to a vector with the correct stride.

  Eigen::Map<const Eigen::VectorXd, Eigen::RowMajor,
             Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>
  value_at(std::size_t _index) {
    std::size_t local_index = index_to_local(_index);
    return Eigen::Map<const Eigen::VectorXd, Eigen::RowMajor,
                      Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>(
        get_coefficients().data() + local_index, get_codom_dim(),
        Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(0, _number_of_glp));
  }

Where index_to_local is a function that we have to develop by reasoning about what is the interval of a given index.

first_coefficient = _index * codom_dim
index coefficient expression
0 0 _index
1 1 _index
... ... ...
nglp -1 nglp -1 _index
nglp ngpl*codom_dim _index *codom_dim
nglp+1 ngpl*codom_dim+1 _index *codom_dim +1
... ... ...
2*nglp-1 ( 2ngpl-1)codom_dim _index *codom_dim + (nglp-1)
2*nglp 2*ngpl*codom_dim