Change the logic used to update the Coefficients in the CubicSpline class

CHANGELOG.md

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Fixed
 - Fixed bug in `yarprobotstatepublisher` that caused segmentation fault each time an unknown joint name was read from the input joint states topic (https://github.com/robotology/idyntree/pull/719)
+- Fixed bug in `CubicSpline()` that causes wrong coefficients calculation when boundary conditions are set (https://github.com/robotology/idyntree/pull/723)
 
 ### Changed
 - By default  iDynTree is compiled as a shared library also on Windows. The `BUILD_SHARED_LIBS` CMake variable can be used to

src/core/include/iDynTree/Core/CubicSpline.h

@@ -24,22 +24,23 @@ namespace iDynTree
         iDynTree::VectorDynSize m_time;
         iDynTree::VectorDynSize m_y;
         iDynTree::VectorDynSize m_T;
-        
+
         double m_v0;
         double m_vf;
         double m_a0;
         double m_af;
-        
+
         bool computePhasesDuration();
         bool computeIntermediateVelocities();
         bool computeCoefficients();
-
-
+
+        bool m_areCoefficientsUpdated;
+
     public:
         CubicSpline();
 
         CubicSpline(unsigned int buffersDimension);
-        
+
         bool setData(const iDynTree::VectorDynSize& time, const iDynTree::VectorDynSize& yData);
         void setInitialConditions(double initialVelocity, double initialAcceleration);
         void setFinalConditions(double finalVelocity, double finalAcceleration);

src/core/src/CubicSpline.cpp

@@ -21,6 +21,7 @@ iDynTree::CubicSpline::CubicSpline()
 ,m_vf(0)
 ,m_a0(0)
 ,m_af(0)
+,m_areCoefficientsUpdated{false}
 {
     m_coefficients.clear();
     m_velocities.resize(0);
@@ -39,6 +40,7 @@ iDynTree::CubicSpline::CubicSpline(unsigned int buffersDimension)
     ,m_vf(0)
     ,m_a0(0)
     ,m_af(0)
+    ,m_areCoefficientsUpdated{false}
 {
 }
 
@@ -48,52 +50,62 @@ bool iDynTree::CubicSpline::setData(const iDynTree::VectorDynSize& time, const i
         std::cerr << "[ERROR][CUBICSPLINE] The input data are empty!" << std::endl;
         return false;
     }
-    
+
     if(time.size() != yData.size()){
         std::cerr << "[ERROR][CUBICSPLINE] The input data are expected to have the same dimension: xData = " << time.size() << ", yData = " << yData.size() << "." <<std::endl;
         return false;
     }
-    
+
     if(time.size() < 2){
         std::cerr << "[ERROR][CUBICSPLINE] At least two data points are needed to compute the spline." << std::endl;
         return false;
     }
 
     m_time.resize(time.size());
     m_y.resize(yData.size());
-    
+
     m_coefficients.resize(time.size()-1);
     m_velocities.resize(time.size());
-    
+
     m_T.resize(time.size()-1);
-    
+
     m_time = time;
 
     m_y = yData;
-
-    return this->computeCoefficients();
+
+    m_areCoefficientsUpdated = false;
+
+    return true;
 }
 
 bool iDynTree::CubicSpline::computeCoefficients()
 {
+    // the coefficients are updated. No need to recompute them
+    if(m_areCoefficientsUpdated){
+        return true;
+    }
+
     m_velocities(0) = m_v0;
     m_velocities(m_velocities.size() - 1) = m_vf;
-    
+
     if(!this->computePhasesDuration())
         return false;
-    
+
     if(m_velocities.size() > 2){
         if(!this->computeIntermediateVelocities())
             return false;
     }
-    
+
     for(size_t i = 0; i < m_coefficients.size(); ++i){
         m_coefficients[i](0) = m_y(i);
         m_coefficients[i](1) = m_velocities(i);
         m_coefficients[i](2) = ( 3*(m_y(i+1) - m_y(i))/m_T(i) - 2*m_velocities(i) - m_velocities(i+1) )/m_T(i);
         m_coefficients[i](3) = ( 2*(m_y(i) - m_y(i+1))/m_T(i) + m_velocities(i) + m_velocities(i+1) )/std::pow(m_T(i), 2);
     }
-
+
+    // The coefficients are now updated.
+    m_areCoefficientsUpdated = true;
+
     return true;
 }
 
@@ -104,13 +116,13 @@ bool iDynTree::CubicSpline::computeIntermediateVelocities()
 
     //Eigen::MatrixXd A(m_velocities.size()-2, m_velocities.size()-2);
     Eigen::VectorXd b(m_velocities.size()-2);
-    
+
     //A.setZero();
-    
+
     //A(0,0) = 2*(m_T(0) + m_T(1));
     A_triplets.push_back(Eigen::Triplet<double>(0, 0, 2*(m_T(0) + m_T(1))));
     b(0) = ( std::pow(m_T(0), 2)*(m_y(2) - m_y(1)) + std::pow(m_T(1), 2)*(m_y(1) - m_y(0)) )*3/(m_T(0)*m_T(1)) - m_T(1)*m_velocities(0);
-    
+
     if(m_velocities.size() > 3){
         //A(0,1) = m_T(0);
         A_triplets.push_back(Eigen::Triplet<double>(0, 1, m_T(0)));
@@ -121,10 +133,10 @@ bool iDynTree::CubicSpline::computeIntermediateVelocities()
             A_triplets.push_back(Eigen::Triplet<double>(i, i, 2*(m_T(i) + m_T(i+1))));
             //A(i,i+1) = m_T(i);
             A_triplets.push_back(Eigen::Triplet<double>(i, i+1, m_T(i)));
-            
+
             b(i) = ( std::pow(m_T(i), 2)*(m_y(i+2) - m_y(i+1)) + std::pow(m_T(i+1), 2)*(m_y(i+1) - m_y(i)) )*3/(m_T(i)*m_T(i+1));
         }
-        size_t T = m_T.size() - 1; 
+        size_t T = m_T.size() - 1;
         //A.bottomRightCorner<1,2>() << m_T(T), 2*(m_T(T) + m_T(T-1));
         A_triplets.push_back(Eigen::Triplet<double>(A.rows() - 1, A.cols() - 2,  m_T(T)));
         A_triplets.push_back(Eigen::Triplet<double>(A.rows() - 1, A.cols() - 1,  2*(m_T(T) + m_T(T-1))));
@@ -136,26 +148,26 @@ bool iDynTree::CubicSpline::computeIntermediateVelocities()
     Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<Eigen::SparseMatrix<double>::StorageIndex>> qrDecomposition;
     qrDecomposition.compute(A);
     iDynTree::toEigen(m_velocities).segment(1, m_velocities.size()-2) = qrDecomposition.solve(b);
-    
+
     return true;
 }
 
 bool iDynTree::CubicSpline::computePhasesDuration()
 {
     for (size_t i = 0; i < m_time.size() - 1; ++i){
-        
+
         m_T(i) = m_time(i+1) - m_time(i);
-        
+
         if(m_T(i) == 0){
             std::cerr << "[ERROR][CUBICSPLINE] Two consecutive points have the same time coordinate." << std::endl; //For stability purposes, the matrix below may not be invertible
             return false;
         }
-        
+
         if(m_T(i) < 0){
             std::cerr << "[ERROR][CUBICSPLINE] The input points are expected to be consecutive, strictly increasing in the time variable." << std::endl; //For stability purposes
             return false;
         }
-        
+
     }
     return true;
 }
@@ -164,18 +176,24 @@ void iDynTree::CubicSpline::setInitialConditions(double initialVelocity, double
 {
     m_v0 = initialVelocity;
     m_a0 = initialAcceleration;
+
+    // The initial condition has been updated. The coefficients have to be recomputed.
+    m_areCoefficientsUpdated = false;
 }
 
 void iDynTree::CubicSpline::setFinalConditions(double finalVelocity, double finalAcceleration)
 {
     m_vf = finalVelocity;
     m_af = finalAcceleration;
+
+    // The initial condition has been updated. The coefficients have to be recomputed.
+    m_areCoefficientsUpdated = false;
 }
 
 double iDynTree::CubicSpline::evaluatePoint(double t)
 {
     double velocity, acceleration;
-    
+
     return evaluatePoint(t, velocity, acceleration);
 }
 
@@ -186,6 +204,14 @@ double iDynTree::CubicSpline::evaluatePoint(double t, double &velocity, double &
         return std::nan("");
     }
 
+    // The coefficients are not updated. It's time to compute them.
+    if(!m_areCoefficientsUpdated){
+        if(!this->computeCoefficients()){
+                std::cerr << "[ERROR][CUBICSPLINE] Unable to compute the internal coefficients of the cubic spline." << std::endl;
+        return std::nan("");
+        }
+    }
+
     if( t < m_time(0) ){
         velocity = m_v0;
         acceleration = m_a0;
@@ -212,4 +238,3 @@ double iDynTree::CubicSpline::evaluatePoint(double t, double &velocity, double &
     acceleration = 2*coeff(2) + 6*coeff(3)*(dt);
     return position;
 }
-