Skip to content

Commit

Permalink
Change the logic used to update the Coefficients in the CubicSpline c…
Browse files Browse the repository at this point in the history 
…lass (#723)

* [Core] Remove trailing whitespaces in CubicSpline* files

* Fix #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
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi authored Aug 14, 2020
1 parent a14592a commit 443b5a6
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 28 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 6 additions & 5 deletions src/core/include/iDynTree/Core/CubicSpline.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
71 changes: 48 additions & 23 deletions src/core/src/CubicSpline.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -39,6 +40,7 @@ iDynTree::CubicSpline::CubicSpline(unsigned int buffersDimension)
,m_vf(0)
,m_a0(0)
,m_af(0)
,m_areCoefficientsUpdated{false}
{
}

Expand All @@ -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;
}

Expand All @@ -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)));
Expand All @@ -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))));
Expand All @@ -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;
}
Expand All @@ -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);
}

Expand All @@ -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;
Expand All @@ -212,4 +238,3 @@ double iDynTree::CubicSpline::evaluatePoint(double t, double &velocity, double &
acceleration = 2*coeff(2) + 6*coeff(3)*(dt);
return position;
}

0 comments on commit 443b5a6

Please sign in to comment.