Add discrete interpollator and fix tests

[EtienneAr]

Hello,

Small PR to add a step function for interpolator.
This will be useful for "interpolating" contacts, or rather choosing the right contact step in the contact sequence based on the timestep and delay.

I also fixed some warnings in the interpolator tests (some variable were declared multiple times...)

[edantec]

I advise against using templates for interpolation functions, as the need is unclear and it complicates the code.

[edantec]

Suggested change

	interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
	interpolator.interpolateConfiguration<Eigen::VectorXd>(delay, timestep, qs, q_interp);

[edantec]

Suggested change

	interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
	interpolator.interpolateConfiguration<Eigen::VectorXd>(delay, timestep, qs, q_interp);

[edantec]

Suggested change

	interpolator.interpolateDiscrete(0.4, 0.1, vs, v_interp);
	interpolator.interpolateDiscrete<Eigen::VectorXd>(0.4, 0.1, vs, v_interp);

[edantec]

In order to compile, self.interpolateConfiguration(delay, timestep, qs, q_interp); should beself.interpolateConfiguration<Eigen::VectorXd>(delay, timestep, qs, q_interp);, same for interpolateState and interpolateLinear.

A proxy for interpolateDiscrete needs to be added.

[EtienneAr]

Ok, I did not see a clean and easy way to handle eigen::ref and have something templated, that doesn't require specifying by hand everytime the template arguments... So I went back to the simple and easy way : everything is expected to be a VectorXd, ecxept contacts that are std::vector

[ManifoldFR]

I would suggest something like this if you want this templated:

    template<int Rows, int Cols, typename MatType
      = Eigen::Matrix<double, Rows, Cols>  
    >
    void interp(
      const double delay,
      const double timestep,
      const std::vector<MatType>& qs,
      Eigen::Ref<MatType> q_interp
    );

[EtienneAr]

So apparently the problems remains

<...>/simple-mpc/tests/interpolator.cpp: In member function 'void interpolator::interpolate::test_method()':
<...>/simple-mpc/tests/interpolator.cpp:38:42: error: no matching function for call to 'simple_mpc::Interpolator::interpolateConfiguration(double&, double&, std::vector<Eigen::Matrix<double, -1, 1> >&, Eigen::VectorXd&)'
   38 |     interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
<...>/simple-mpc/include/simple-mpc/interpolator.hxx:29:10: note: candidate: 'template<class Type, int Rows, int Cols, class MatType> void simple_mpc::Interpolator::interpolateConfiguration(double, double, const std::vector<MatType>&, Eigen::Ref<MatType>)'
   29 |     void interpolateConfiguration(
      |          ^~~~~~~~~~~~~~~~~~~~~~~~
<...>/simple-mpc/include/simple-mpc/interpolator.hxx:29:10: note:   template argument deduction/substitution failed:
<...>/simple-mpc/tests/interpolator.cpp:38:42: note:   'Eigen::Matrix<double, -1, 1>' is not derived from 'Eigen::Ref<MatType>'
   38 |     interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

[ManifoldFR]

Can you show the calling code and the current implementation?

[EtienneAr]

  pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model);
  double timestep = 0.01;
  Interpolator interpolator = Interpolator(model);

  // Test configuration interpolation method
  {
    std::vector<Eigen::VectorXd> qs;
    for (std::size_t i = 0; i < 4; i++)
    {
      Eigen::VectorXd q0(model.nq);
      Eigen::VectorXd q1(model.nq);
      q0 = pinocchio::neutral(model);
      Eigen::VectorXd dq(model.nv);
      dq.setRandom();
      pinocchio::integrate(model, q0, dq, q1);
      qs.push_back(q1);
    }
    Eigen::VectorXd q_interp(model.nq);

    double delay = 0.02;
    interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);

[ManifoldFR]

What about the interpolateConfiguration function itself?

[EtienneAr]

    template<typename Type, int Rows, int Cols, typename MatType = Eigen::Matrix<Type, Rows, Cols>>
    void interpolateConfiguration(
      const double delay, const double timestep, const std::vector<MatType> & qs, Eigen::Ref<MatType> q_interp)
    {
      assert(("Configuration is not of the right size", qs[0].size() == model_.nq));

      // Compute the time knot corresponding to the current delay
      size_t step_nb = static_cast<size_t>(delay / timestep);
      double step_progress = (delay - (double)step_nb * timestep) / timestep;

      // Interpolate configuration trajectory
      if (step_nb >= qs.size() - 1)
        q_interp = qs.back();
      else
      {
        q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress);
      }
    }

I tried with and without the type template

[ManifoldFR]

Strange. You could remove MatType as an argument and write Eigen::Matrix<...> everywhere in its place. Perhaps this messed up the function template parameter deduction...

[ManifoldFR]

not perfect, not terrible

approved (stuff can get optimised later)