From 443b5a6728118fdfd91ffbe3dec9a61abab65d83 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 15 Aug 2020 00:54:33 +0200 Subject: [PATCH] Change the logic used to update the Coefficients in the CubicSpline class (#723) * [Core] Remove trailing whitespaces in CubicSpline* files * Fix robotology/idyntree#722 * introduce `m_areCoefficientsUpdated` flag to check if the coefficients are updated * compute the internal coefficients only if it is necessary * Update CHANGELOG.md * Add braces in the if conditions in CubicSpline::setData() method --- CHANGELOG.md | 1 + src/core/include/iDynTree/Core/CubicSpline.h | 11 +-- src/core/src/CubicSpline.cpp | 71 +++++++++++++------- 3 files changed, 55 insertions(+), 28 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 99f611321d5..15998c3a5d4 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/core/include/iDynTree/Core/CubicSpline.h b/src/core/include/iDynTree/Core/CubicSpline.h index 14a9cb292d5..963eec0bca5 100644 --- a/src/core/include/iDynTree/Core/CubicSpline.h +++ b/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); diff --git a/src/core/src/CubicSpline.cpp b/src/core/src/CubicSpline.cpp index 8cd103e26d9..225a4e1b542 100644 --- a/src/core/src/CubicSpline.cpp +++ b/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,12 +50,12 @@ 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() << "." <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(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(0, 1, m_T(0))); @@ -121,10 +133,10 @@ bool iDynTree::CubicSpline::computeIntermediateVelocities() A_triplets.push_back(Eigen::Triplet(i, i, 2*(m_T(i) + m_T(i+1)))); //A(i,i+1) = m_T(i); A_triplets.push_back(Eigen::Triplet(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(A.rows() - 1, A.cols() - 2, m_T(T))); A_triplets.push_back(Eigen::Triplet(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::COLAMDOrdering::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; } -