Analyze the time profiling of WBD methods

[GiulioRomualdi]

I'm trying to run WBD at 1khz. As discussed in ami-iit/robots-configuration#10 I increased the rate of all the devices used by wbd.

In the context of xprize I wrote a simple time profiler for wbd e44b223. Running the profiler with wbd at 1khz I obtain the following result

timer name	average time (s)	deadline misses	last deadline miss time (s)
publish	0.000395487473	0	0
compute forces	0.000179018516	0	0
calibration	4.18783e-07	0	0
kinematics	2.4863994e-05	0	0
remove offset	0.000285472475	0	0
contact points	1.5841566e-05	0	0
read sensors	2.9965689e-05	0	0
all	0.000937095005	8351	0.001013504

Accordingly to the table there seems that wbd spends considerably amount of time in publishing and removing offsets (low pass filtering).
Checking with @prashanthr05 we noticed that a a possible bottleneck is due to yarp-to-idyntree copy

whole-body-estimators/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Lines 2183 to 2292 in bf7d5ff

	void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
	{
	filters.updateCutOffFrequency(settings.forceTorqueFilterCutoffInHz,
	settings.imuFilterCutoffInHz,
	settings.jointVelFilterCutoffInHz,
	settings.jointAccFilterCutoffInHz);
	// Filter and remove offset fromn F/T sensors
	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]);
	// Filter the data
	iDynTree::toYarp(rawFTMeasureWithOffsetRemoved,filters.bufferYarp6);
	// Run the filter
	const yarp::sig::Vector & outputFt = filters.forcetorqueFilters[ft]->filt(filters.bufferYarp6);
	iDynTree::Wrench filteredFTMeasure;
	iDynTree::toiDynTree(outputFt,filteredFTMeasure);
	filteredSensorMeasurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,filteredFTMeasure);
	}
	if( settings.estimateJointVelocityAcceleration )
	{
	iDynTree::VectorDynSize kfState;
	kfState.resize(estimator.model().getNrOfDOFs()*3);
	iDynTree::VectorDynSize measurement(estimator.model().getNrOfDOFs());
	iDynTree::toEigen(measurement) = iDynTree::toEigen(jointPos);
	if (!filters.jntVelAccKFFilter->kfPredict())
	{
	yError() << " wholeBodyDynamics : kf predict step failed ";
	}
	if (!filters.jntVelAccKFFilter->kfSetMeasurementVector(measurement))
	{
	yError() << " wholeBodyDynamics : kf cannot set measurement ";
	}
	if (!filters.jntVelAccKFFilter->kfUpdate())
	{
	yError() << " wholeBodyDynamics : kf update step failed ";
	}
	// The first estimator.model().getNrOfDOFs() values of the state are the joint positions
	// the following estimator.model().getNrOfDOFs() values are the joint velocities
	// the last estimator.model().getNrOfDOFs() values are the joint accelerations
	filters.jntVelAccKFFilter->kfGetStates(kfState);
	iDynTree::toEigen(jointPosKF) = iDynTree::toEigen(kfState).head(estimator.model().getNrOfDOFs());
	if( settings.useJointVelocity )
	{
	iDynTree::toEigen(jointVelKF) = iDynTree::toEigen(kfState).segment(estimator.model().getNrOfDOFs(),estimator.model().getNrOfDOFs());
	}
	if( settings.useJointAcceleration )
	{
	iDynTree::toEigen(jointAccKF) = iDynTree::toEigen(kfState).tail(estimator.model().getNrOfDOFs());
	}
	}
	// 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);
	}
	// 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);
	}
	// 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);
	// For now we just assume that the angular acceleration is zero
	filteredIMUMeasurements.angularAcc.zero();
	}
	}

To avoid these copies we could replace the yarp vectors in ctrLibRT filters with eigen refs/vectors.

cc @prashanthr05 @HosameldinMohamed @traversaro @isorrentino @S-Dafarra

[traversaro]

Checking with @prashanthr05 we noticed that a a possible bottleneck is due to yarp-to-idyntree copy

Not sure if that copy can be so expensive, even considering that the buffers are already allocated and of the right size. Anyhow, worth investigating.