Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Attempt to improve the performances of the ctrlLibRT library #143

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Attempt to improve the performances of the ctrlLibRT library #143

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5b5ab96
Remove yarp depenency in ctrlLibRT
GiulioRomualdi Mar 8, 2022
7058fe2
Reduce the number of copies required to filters the readouts retrieve…
GiulioRomualdi Mar 8, 2022
5329f02
Bugfix in WholeBodyDynamicsDevice filtering
GiulioRomualdi Mar 8, 2022
f4dc60d
Update WholeBodyDynamicsDevice documentation
GiulioRomualdi Mar 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 21 additions & 42 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2188,22 +2188,23 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
settings.jointAccFilterCutoffInHz);

// Filter and remove offset fromn F/T sensors
iDynTree::Wrench rawFTMeasure;
iDynTree::Wrench rawFTMeasureWithOffsetRemoved;
iDynTree::Wrench filteredFTMeasure;
for(size_t ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
iDynTree::Wrench rawFTMeasure;
rawSensorsMeasurements.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,rawFTMeasure);

iDynTree::Wrench rawFTMeasureWithOffsetRemoved = ftProcessors[ft].filt(rawFTMeasure,tempMeasurements[ft]);
rawFTMeasureWithOffsetRemoved = ftProcessors[ft].filt(rawFTMeasure,tempMeasurements[ft]);

// Filter the data
iDynTree::toYarp(rawFTMeasureWithOffsetRemoved,filters.bufferYarp6);
// // Filter the data
filters.bufferEigen6.head<3>() = iDynTree::toEigen(rawFTMeasureWithOffsetRemoved.getLinearVec3());
filters.bufferEigen6.tail<3>() = iDynTree::toEigen(rawFTMeasureWithOffsetRemoved.getAngularVec3());

// Run the filter
const yarp::sig::Vector & outputFt = filters.forcetorqueFilters[ft]->filt(filters.bufferYarp6);

iDynTree::Wrench filteredFTMeasure;

iDynTree::toiDynTree(outputFt,filteredFTMeasure);
Eigen::Ref<const Eigen::VectorXd> outputFt = filters.forcetorqueFilters[ft]->filt(filters.bufferEigen6);
iDynTree::toEigen(filteredFTMeasure.getLinearVec3()) = outputFt.head<3>();
iDynTree::toEigen(filteredFTMeasure.getAngularVec3()) = outputFt.tail<3>();

filteredSensorMeasurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,filteredFTMeasure);
}
Expand Down Expand Up @@ -2254,37 +2255,20 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
// Filter joint vel
if( settings.useJointVelocity )
{
iDynTree::toYarp(jointVel,filters.bufferYarpDofs);

const yarp::sig::Vector & outputJointVel = filters.jntVelFilter->filt(filters.bufferYarpDofs);

iDynTree::toiDynTree(outputJointVel,jointVel);
iDynTree::toEigen(jointVel) = filters.jntVelFilter->filt(iDynTree::toEigen(jointVel));
}

// Filter joint acc
if( settings.useJointAcceleration )
{
iDynTree::toYarp(jointAcc,filters.bufferYarpDofs);

const yarp::sig::Vector & outputJointAcc = filters.jntAccFilter->filt(filters.bufferYarpDofs);

iDynTree::toiDynTree(outputJointAcc,jointAcc);
iDynTree::toEigen(jointAcc) = filters.jntAccFilter->filt(iDynTree::toEigen(jointAcc));
}

// Filter IMU Sensor
if( settings.kinematicSource == IMU )
{
iDynTree::toYarp(rawIMUMeasurements.linProperAcc,filters.bufferYarp3);

const yarp::sig::Vector & outputLinAcc = filters.imuLinearAccelerationFilter->filt(filters.bufferYarp3);

iDynTree::toiDynTree(outputLinAcc,filteredIMUMeasurements.linProperAcc);

iDynTree::toYarp(rawIMUMeasurements.angularVel,filters.bufferYarp3);

const yarp::sig::Vector & outputAngVel = filters.imuAngularVelocityFilter->filt(filters.bufferYarp3);

iDynTree::toiDynTree(outputAngVel,filteredIMUMeasurements.angularVel);
iDynTree::toEigen(filteredIMUMeasurements.linProperAcc) = filters.imuLinearAccelerationFilter->filt(iDynTree::toEigen(rawIMUMeasurements.linProperAcc));
iDynTree::toEigen(filteredIMUMeasurements.angularVel) = filters.imuAngularVelocityFilter->filt(iDynTree::toEigen(rawIMUMeasurements.angularVel));

// For now we just assume that the angular acceleration is zero
filteredIMUMeasurements.angularAcc.zero();
Expand Down Expand Up @@ -3430,10 +3414,7 @@ wholeBodyDynamicsDeviceFilters::wholeBodyDynamicsDeviceFilters(): imuLinearAccel
forcetorqueFilters(0),
jntVelFilter(0),
jntAccFilter(0),
jntVelAccKFFilter(nullptr),
bufferYarp3(0),
bufferYarp6(0),
bufferYarpDofs(0)
jntVelAccKFFilter(nullptr)
{

}
Expand Down Expand Up @@ -3551,26 +3532,24 @@ void wholeBodyDynamicsDeviceFilters::init(int nrOfFTSensors,
double periodInSeconds)
{
// Allocate buffers
bufferYarp3.resize(3,0.0);
bufferYarp6.resize(6,0.0);
bufferYarpDofs.resize(nrOfDOFsProcessed,0.0);
const Eigen::VectorXd dofsDummy = Eigen::VectorXd::Zero(nrOfDOFsProcessed);

imuLinearAccelerationFilter =
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForIMUInHz,periodInSeconds,bufferYarp3);
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForIMUInHz,periodInSeconds,Eigen::Vector3d::Zero());
imuAngularVelocityFilter =
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForIMUInHz,periodInSeconds,bufferYarp3);
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForIMUInHz,periodInSeconds,Eigen::Vector3d::Zero());

forcetorqueFilters.resize(nrOfFTSensors);
for(int ft_numeric = 0; ft_numeric < nrOfFTSensors; ft_numeric++ )
{
forcetorqueFilters[ft_numeric] =
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForFTInHz,periodInSeconds,bufferYarp6);
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForFTInHz,periodInSeconds,Eigen::Matrix<double,6,1>::Zero());
}

jntVelFilter =
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForJointVelInHz,periodInSeconds,bufferYarpDofs);
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForJointVelInHz,periodInSeconds,dofsDummy);
jntAccFilter =
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForJointAccInHz,periodInSeconds,bufferYarpDofs);
new iCub::ctrl::realTime::FirstOrderLowPassFilter(initialCutOffForJointAccInHz,periodInSeconds,dofsDummy);
}


Expand Down
10 changes: 2 additions & 8 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,14 +112,8 @@ class wholeBodyDynamicsDeviceFilters
///< KF filter for Joint velocity and accelerations
std::unique_ptr<iDynTree::DiscreteKalmanFilterHelper> jntVelAccKFFilter;

///< Yarp vector buffer of dimension 3
yarp::sig::Vector bufferYarp3;

///< Yarp vector buffer of dimension 6
yarp::sig::Vector bufferYarp6;

///< Yarp vector buffer of dimension dofs
yarp::sig::Vector bufferYarpDofs;
///< Temporary Eigen vector buffer of dimension 6
Eigen::Matrix<double, 6, 1> bufferEigen6;
};

/**
Expand Down
1 change: 0 additions & 1 deletion libraries/ctrlLibRT/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC

include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR})
target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES})

set_property(TARGET ${PROJECT_NAME} PROPERTY PUBLIC_HEADER ${${PROJECT_NAME}_HELPERS_SRCS})

Expand Down
158 changes: 79 additions & 79 deletions libraries/ctrlLibRT/include/ctrlLibRT/filters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,8 @@
#ifndef RT_FILTERS_H
#define RT_FILTERS_H

#include<Eigen/Dense>

#include <yarp/sig/Vector.h>

#include <Eigen/Dense>
#include <memory>

namespace iCub
{
Expand All @@ -54,7 +52,7 @@ class Filter
protected:
Eigen::VectorXd b;
Eigen::VectorXd a;
yarp::sig::Vector y;
Eigen::VectorXd y;

Eigen::MatrixXd uold; ///< Matrix of past inputs: each column is a past sample
int uold_last_column_sample;
Expand All @@ -76,77 +74,82 @@ class Filter
* @param y0 initial output.
* @note den[0] shall not be 0.
*/
Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den,
const yarp::sig::Vector &y0);
Filter(const Eigen::Ref<const Eigen::VectorXd>& num,
const Eigen::Ref<const Eigen::VectorXd>& den,
const Eigen::Ref<const Eigen::VectorXd>& y0);

/**
* Internal state reset.
* @param y0 new internal state.
*/
void init(const yarp::sig::Vector &y0);
/**
* Internal state reset.
* @param y0 new internal state.
*/
void init(const Eigen::Ref<const Eigen::VectorXd>& y0);

/**
* Internal state reset for filter with zero gain.
* @param y0 new internal state.
* @param u0 expected next input.
* @note The gain of a digital filter is the sum of the coefficients of its
* numerator divided by the sum of the coefficients of its denumerator.
*/
void init(const yarp::sig::Vector &y0, const yarp::sig::Vector &u0);
/**
* Internal state reset for filter with zero gain.
* @param y0 new internal state.
* @param u0 expected next input.
* @note The gain of a digital filter is the sum of the coefficients of its
* numerator divided by the sum of the coefficients of its denumerator.
*/
void
init(const Eigen::Ref<const Eigen::VectorXd>& y0, const Eigen::Ref<const Eigen::VectorXd>& u0);

/**
* Returns the current filter coefficients.
* @param num vector of numerator elements returned as increasing
* power of z^-1.
* @param den vector of denominator elements returned as
* increasing power of z^-1.
*/
void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den);
/**
* Returns the current filter coefficients.
* @param num vector of numerator elements returned as increasing
* power of z^-1.
* @param den vector of denominator elements returned as
* increasing power of z^-1.
*/
void getCoeffs(Eigen::Ref<Eigen::VectorXd> num, Eigen::Ref<Eigen::VectorXd> den);

/**
* Sets new filter coefficients.
* @param num vector of numerator elements given as increasing
* power of z^-1.
* @param den vector of denominator elements given as increasing
* power of z^-1.
* @note den[0] shall not be 0.
* @note the internal state is reinitialized to the current
* output.
*/
void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
/**
* Sets new filter coefficients.
* @param num vector of numerator elements given as increasing
* power of z^-1.
* @param den vector of denominator elements given as increasing
* power of z^-1.
* @note den[0] shall not be 0.
* @note the internal state is reinitialized to the current
* output.
*/
void setCoeffs(const Eigen::Ref<const Eigen::VectorXd>& num,
const Eigen::Ref<const Eigen::VectorXd>& den);

/**
* Modifies the values of existing filter coefficients without
* varying their lengths.
* @param num vector of numerator elements given as increasing
* power of z^-1.
* @param den vector of denominator elements given as increasing
* power of z^-1.
* @return true/false on success/fail.
* @note den[0] shall not be 0.
* @note the adjustment is carried out iff num.size() and
* den.size() match the existing numerator and denominator
* lengths.
*/
bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
/**
* Modifies the values of existing filter coefficients without
* varying their lengths.
* @param num vector of numerator elements given as increasing
* power of z^-1.
* @param den vector of denominator elements given as increasing
* power of z^-1.
* @return true/false on success/fail.
* @note den[0] shall not be 0.
* @note the adjustment is carried out iff num.size() and
* den.size() match the existing numerator and denominator
* lengths.
*/
bool adjustCoeffs(const Eigen::Ref<const Eigen::VectorXd>& num,
const Eigen::Ref<const Eigen::VectorXd>& den);

/**
* Performs filtering on the actual input.
* @param u reference to the actual input.
* @return a reference the corresponding output.
* @note the returned reference is valid till any new call to filt.
*/
const yarp::sig::Vector & filt(const yarp::sig::Vector &u);
/**
* Performs filtering on the actual input.
* @param u reference to the actual input.
* @return a reference the corresponding output.
* @note the returned reference is valid till any new call to filt.
*/
Eigen::Ref<const Eigen::VectorXd> filt(const Eigen::Ref<const Eigen::VectorXd>& u);

/**
* Return the reference to the current filter output.
* @return reference to the filter output.
*/
const yarp::sig::Vector & output() const { return y; }
/**
* Return the reference to the current filter output.
* @return reference to the filter output.
*/
Eigen::Ref<const Eigen::VectorXd> output() const
{
return y;
}
};



/**
* \ingroup Filters
*
Expand All @@ -157,10 +160,10 @@ class Filter
class FirstOrderLowPassFilter
{
protected:
Filter *filter; // low pass filter
std::unique_ptr<Filter> filter; // low pass filter
double fc; // cut frequency
double Ts; // sample time
yarp::sig::Vector y; // filter current output
Eigen::VectorXd y; // filter current output

void computeCoeff();

Expand All @@ -172,18 +175,12 @@ class FirstOrderLowPassFilter
* @param y0 initial output.
*/
FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime,
const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));

/**
* Destructor.
*/
~FirstOrderLowPassFilter();

const Eigen::Ref<const Eigen::VectorXd> &y0=Eigen::VectorXd::Zero(1));
/**
* Internal state reset.
* @param y0 new internal state.
*/
void init(const yarp::sig::Vector &y0);
void init(const Eigen::Ref<const Eigen::VectorXd> &y0);

/**
* Change the cut frequency of the filter.
Expand Down Expand Up @@ -214,13 +211,16 @@ class FirstOrderLowPassFilter
* @param u reference to the actual input.
* @return the corresponding output.
*/
const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
Eigen::Ref<const Eigen::VectorXd> filt(const Eigen::Ref<const Eigen::VectorXd> &u);

/**
* Return current filter output.
* @return the filter output.
*/
const yarp::sig::Vector& output() const { return y; }
Eigen::Ref<const Eigen::VectorXd> output() const
{
return y;
}
};


Expand Down
Loading