Steering method for spline path for on non-vector spaces

[jmirabel]

steeringMethod::Spline::steer is not tested and not working correctly for non-vector space.

At

hpp-core/src/path/spline.cc

Line 386 in e0363c5

if (!robot_->configSpace()->isVectorSpace()) {

the spline makes use of dIntegrate_dv

  BasisFunctionVector_t basisFunc;
  const value_type u =
      (length() == 0 ? 0 : (s - paramRange().first) / paramLength());
  basisFunctionDerivative(order, u, basisFunc);
  res.noalias() = parameters_.transpose() * basisFunc;

  if (!robot_->configSpace()->isVectorSpace()) {
    basisFunctionDerivative(0, u, basisFunc);
    vector_t v(parameters_.transpose() * basisFunc);
    // true means: res <- Jdiff * res
    base_.space()->dIntegrate_dv<pinocchio::DerivativeTimesInput>(base_, v,
                                                                  res);
  }

and this isn't used when computing the spline coefficients from desired velocities and accelerations.

At the moment, the coefficient are computed by solving the linear equation A * coeffs = b.
Each row correspond to one constraint on the position, velocity and/or acceleration at the start or end of the trajectory.

The problem shouldn't be too hard to solve since the extremum points are know so v in the code snippet above is actually independent of the spline coefficient.

It is worth noting that the comment in the same function

  // For non vector space, it is not possible to compute the derivatives
  // at a higher order. At the d2+/dv2 is not available for SE(n) and SO(n).

means one cannot use steeringMethod::Spline::steer for 5th order spline on SE(n)/SO(n).

[florent-lamiraux]

Can we close this issue ?

[jmirabel]

This issue is stale as it is very unlikely that we will work on that but the bug is still here. Up to you to decide what to do with stale issues.

[florent-lamiraux]

Thank you for the update. We will keep it open.