Skip to content

Commit

Permalink
Add kalman filter joint velocity and acceleration estimation (#139)
Browse files Browse the repository at this point in the history 
* Add kalman filter joint velocity and acceleration estimation
Modify joint torque and external wrench estimation setting the correct joint velocity and acceleration when the KF estimation is used
Modify configuration files adding paramenters required for the KF inititalization

* Remove unused variables and includes

* Use only joint position for kf measurement

* Modify README

* Change default value for estimateJointVelocityAcceleration parameter

* Use unique_ptr for kalman filter object

* Modify documentation

* Update devices/wholeBodyDynamics/README.md

Co-authored-by: Prashanth Ramadoss <[email protected]>

* Update CHANGELOG

* Fix covariances in configuration files
Update documentation

Co-authored-by: Prashanth Ramadoss <[email protected]>
  • Loading branch information
@isorrentino @prashanthr05
isorrentino and prashanthr05 authored Feb 23, 2022
1 parent 6841cdc commit bf7d5ff
Show file tree
Hide file tree
Showing 13 changed files with 569 additions and 22 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).

## Unreleased
### Added
- Add a steady-state Kalman filter with a null jerk model for the joint velocity and acceleration estimation in `WholeBodyDynamics`. (See [!139](https://github.com/robotology/whole-body-estimators/pull/139)).

### Changed
- Updated the `yarp rpc` command `calibStandingWithJetsiRonCubMk1` to be `calibStandingWithJetsiRonCub`, in order to perform `calibStanding` for the models `iRonCub-Mk1` and `iRonCub-Mk1_1` (See [!136](https://github.com/robotology/whole-body-estimators/pull/136)).
Expand Down
1 change: 0 additions & 1 deletion devices/wholeBodyDynamics/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@ find_package(YARP REQUIRED)
find_package(ICUB REQUIRED)
find_package(iDynTree REQUIRED)


yarp_prepare_plugin(wholebodydynamics CATEGORY device
TYPE yarp::dev::WholeBodyDynamicsDevice
INCLUDE "WholeBodyDynamicsDevice.h"
Expand Down
6 changes: 5 additions & 1 deletion devices/wholeBodyDynamics/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
| streamFilteredFT | - | bool | - | false | No | Select if the filtered and offset removed forces will be streamed or not. The name of the ports have the following syntax: portname=(portPrefix+"/filteredFT/"+sensorName). Example: "myPrefix/filteredFT/l_leg_ft_sensor" | The value streamed by this ports is affected by the secondary calibration matrix, the estimated offset and temperature coefficients ( if any ). |
| publishNetExternalWrenches | - | bool | - | false | No | Flag to stream the net external wrenches acting on each link on the port portPrefix+"/netExternalWrenches:o". The content is a bottle of n pairs, where n is the number of links. The first element of each pair is the name of the link. The second element is yet another list of 6 elements containing the value of the net external wrench, defined in the link frame. The first three elements are the linear part, while the other three are the angular part.| |
| useSkinForContacts | - | bool | - | true | No | Flag to skip using tactile skin sensors for updating contact points and external force (pressure) information | |
| estimateJointVelocityAcceleration | - | bool | - | false | No | Flag to estimate the joint velocities and accelerations using Kalman filter. If true, the same measurements from the low level are ignored. | |
| processNoiseCovariance | - | vector of doubles | - | - | Yes if 'estimateJointVelocityAcceleration' is true | It should contain 3 values per each joint, in total number_of_joints*3 values. The joint order follows the one in axesNames. | |
| measurementNoiseCovariance | - | vector of doubles | - | - | Yes if 'estimateJointVelocityAcceleration' is true | It should contain number_of_joints values. The joint order follows the one in axesNames. | |
| stateCovariance | - | vector of doubles | - | - | Yes if 'estimateJointVelocityAcceleration' is true | It should contain 3 values per each joint, in total number_of_joints*3 values. The joint order follows the one in axesNames. | |
| IDYNTREE_SKINDYNLIB_LINKS | - | group | - | - | Yes | Group describing the mapping between link names and skinDynLib identifiers. | |
| | linkName_1 | string (name of a link in the model) | - | - | Yes | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex | |
| | ... | string (name of a link in the model) | - | - | Yes | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex | |
Expand Down Expand Up @@ -225,7 +229,7 @@ For a detailed explanation on their usage, please see the document [Using temper
</device>
</devices>
```

### RPC commands
You can access the device while running via `YARP RPC`. You can run the following command to access into the RPC mode of the device.

Expand Down
251 changes: 243 additions & 8 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,35 @@ bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std
return true;
}

bool getConfigParamsAsList(const yarp::os::Searchable& config,std::string propertyName , iDynTree::VectorDynSize & list, bool isRequired)
{
yarp::os::Property prop;
prop.fromString(config.toString().c_str());
yarp::os::Bottle *propNames=prop.find(propertyName).asList();
if(propNames==nullptr)
{
if(isRequired)
{
yError() <<"WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \" should be followed by a list\n";
}
return false;
}

list.resize(propNames->size());
for(auto elem=0u; elem < propNames->size(); elem++)
{
const auto value = propNames->get(elem);
if( !value.isFloat64() && !value.isInt32() )
{
yError() << "WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \". Expected double or integer array.\n";
return false;
}
list(elem) = value.asFloat64();
}

return true;
}

bool getUsedDOFsList(os::Searchable& config, std::vector<std::string> & usedDOFs)
{
bool required{true};
Expand All @@ -194,7 +223,6 @@ bool getGravityCompensationDOFsList(os::Searchable& config, std::vector<std::str
return getConfigParamsAsList(config,"gravityCompensationAxesNames",gravityCompensationDOFs, required);
}


bool WholeBodyDynamicsDevice::openRemapperControlBoard(os::Searchable& config)
{
// Pass to the remapper just the relevant parameters (axesList)
Expand Down Expand Up @@ -824,6 +852,11 @@ void WholeBodyDynamicsDevice::resizeBuffers()
this->estimatedJointTorques.resize(estimator.model());
this->estimatedJointTorquesYARP.resize(this->estimatedJointTorques.size(),0.0);
this->estimateExternalContactWrenches.resize(estimator.model());
this->jointPosKF.resize(estimator.model());
this->jointVelKF.resize(estimator.model());
this->jointAccKF.resize(estimator.model());
jointVelKF.zero();
jointAccKF.zero();

// Resize F/T stuff
size_t nrOfFTSensors = estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
Expand Down Expand Up @@ -1010,6 +1043,18 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config)
settings.useJointAcceleration = prop.find(useJointAccelerationOptionName.c_str()).asBool();
}

std::string estimateJointVelocityAccelerationOptionName = "estimateJointVelocityAcceleration";
if( !(prop.check(estimateJointVelocityAccelerationOptionName.c_str()) && prop.find(estimateJointVelocityAccelerationOptionName.c_str()).isBool()) )
{
yWarning() << "wholeBodyDynamics: estimateVelocityAccelerationOptionName bool parameter missing, please specify it.";
yWarning() << "wholeBodyDynamics: setting estimateVelocityAccelerationOptionName to the default value of true, but this is a deprecated behaviour that will be removed in the future.";
settings.estimateJointVelocityAcceleration = false;
}
else
{
settings.estimateJointVelocityAcceleration = prop.find(estimateJointVelocityAccelerationOptionName.c_str()).asBool();
}

// Check for measurements low pass filter cutoff frequencies
if (!applyLPFSettingsFromConfig(prop, "imuFilterCutoffInHz"))
{
Expand Down Expand Up @@ -1421,6 +1466,18 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config)
// resize internal buffers
this->resizeBuffers();

// initialize kalman filter
if(settings.estimateJointVelocityAcceleration)
{
filters.jntVelAccKFFilter = std::make_unique<iDynTree::DiscreteKalmanFilterHelper>();

if( !filters.initKalmanFilter(config, filters.jntVelAccKFFilter, getPeriod(), estimator.model().getNrOfDOFs()))
{
yError() << "wholeBodyDynamics: unable to init kalman filter.";
return false;
}
}

// Open settings related to gravity compensation (we need the estimator to be open)
ok = this->loadGravityCompensationSettingsFromConfig(config);
if( !ok )
Expand Down Expand Up @@ -2075,7 +2132,6 @@ void WholeBodyDynamicsDevice::readSensors()
}

// At the moment we are assuming that all joints are revolute

if( settings.useJointVelocity )
{
ok = remappedControlBoardInterfaces.encs->getEncoderSpeeds(jointVel.data());
Expand Down Expand Up @@ -2104,7 +2160,6 @@ void WholeBodyDynamicsDevice::readSensors()

// Convert from degrees (used on wire by YARP) to radians (used by iDynTree)
convertVectorFromDegreesToRadians(jointAcc);

}
else
{
Expand Down Expand Up @@ -2153,6 +2208,49 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
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 )
{
Expand Down Expand Up @@ -2201,8 +2299,16 @@ void WholeBodyDynamicsDevice::updateKinematics()
// Hardcode for the meanwhile
iDynTree::FrameIndex imuFrameIndex = estimator.model().getFrameIndex(settings.imuFrameName);

estimator.updateKinematicsFromFloatingBase(jointPos,jointVel,jointAcc,imuFrameIndex,
filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
if(settings.estimateJointVelocityAcceleration)
{
estimator.updateKinematicsFromFloatingBase(jointPos,jointVelKF,jointAccKF,imuFrameIndex,
filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
}
else
{
estimator.updateKinematicsFromFloatingBase(jointPos,jointVel,jointAcc,imuFrameIndex,
filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
}

if( m_gravityCompensationEnabled )
{
Expand All @@ -2222,7 +2328,14 @@ void WholeBodyDynamicsDevice::updateKinematics()
gravity(1) = settings.fixedFrameGravity.y;
gravity(2) = settings.fixedFrameGravity.z;

estimator.updateKinematicsFromFixedBase(jointPos,jointVel,jointAcc,fixedFrameIndex,gravity);
if( settings.estimateJointVelocityAcceleration )
{
estimator.updateKinematicsFromFixedBase(jointPos,jointVelKF,jointAccKF,fixedFrameIndex,gravity);
}
else
{
estimator.updateKinematicsFromFixedBase(jointPos,jointVel,jointAcc,fixedFrameIndex,gravity);
}

if( m_gravityCompensationEnabled )
{
Expand Down Expand Up @@ -2611,7 +2724,10 @@ void WholeBodyDynamicsDevice::publishExternalWrenches()
// Update kinDynComp model
iDynTree::Vector3 dummyGravity;
dummyGravity.zero();
this->kinDynComp.setRobotState(this->jointPos,this->jointVel,dummyGravity);
if ( settings.estimateJointVelocityAcceleration )
this->kinDynComp.setRobotState(this->jointPos,this->jointVelKF,dummyGravity);
else
this->kinDynComp.setRobotState(this->jointPos,this->jointVel,dummyGravity);
}

if (enablePublishNetExternalWrenches || this->outputWrenchPorts.size() > 0)
Expand Down Expand Up @@ -2896,7 +3012,7 @@ bool WholeBodyDynamicsDevice::setupCalibrationWithVerticalForcesOnTheFeetAndJets
{
yError() << "Model name is invalid, choose either mk1 or mk1.1" ;
return false;
}
}

// Check if the iRonCub-XX jets frames exist
std::string leftArmJetFrame = {"l_arm_jet_turbine"};
Expand Down Expand Up @@ -3262,6 +3378,15 @@ bool WholeBodyDynamicsDevice::setUseOfJointAccelerations(const bool enable)
return true;
}

bool WholeBodyDynamicsDevice::setUseOfJointVelocityAccelerationEstimation(const bool enable)
{
std::lock_guard<std::mutex> guard(this->deviceMutex);

this->settings.estimateJointVelocityAcceleration = enable;

return true;
}

std::string WholeBodyDynamicsDevice::getCurrentSettingsString()
{
std::lock_guard<std::mutex> guard(this->deviceMutex);
Expand Down Expand Up @@ -3305,13 +3430,118 @@ wholeBodyDynamicsDeviceFilters::wholeBodyDynamicsDeviceFilters(): imuLinearAccel
forcetorqueFilters(0),
jntVelFilter(0),
jntAccFilter(0),
jntVelAccKFFilter(nullptr),
bufferYarp3(0),
bufferYarp6(0),
bufferYarpDofs(0)
{

}

bool wholeBodyDynamicsDeviceFilters::initCovarianceMatrix(const yarp::os::Searchable &config, std::string parameterName, iDynTree::MatrixDynSize & matrix)
{
iDynTree::VectorDynSize covariances;
if(!getConfigParamsAsList(config, parameterName, covariances, true))
{
yError() << " wholeBodyDynamics : covariance vector not valid. ";
return false;
}

if( (matrix.cols() != matrix.rows()) || (covariances.size() != matrix.cols()) )
{
yError() << " wholeBodyDynamics : size of covariance vector does not match covariance matrix size. Expected " << matrix.cols() << " . Read " << covariances.size();
return false;
}

matrix.zero();
iDynTree::toEigen(matrix).diagonal() = iDynTree::toEigen(covariances);

return true;
}

bool wholeBodyDynamicsDeviceFilters::initKalmanFilter(const yarp::os::Searchable &config, std::unique_ptr<iDynTree::DiscreteKalmanFilterHelper> &kf, double periodInSeconds, int nrOfDOFsProcessed)
{
// Steady-state Kalman Filter with a null jerk model.
//
// The state vector contains the joint positions, velocities and accelerations.
// x = [q0,...,qn,dq0,...,dqn,ddq0,...,ddqn]
//
// There is no input. The measures are the joint positions.
// y = [q0,...,qn]
//
// State space model:
// x{k+1} = A x{k}
// y{k} = C x{k}

iDynTree::MatrixDynSize A(3*nrOfDOFsProcessed, 3*nrOfDOFsProcessed);
A.zero();
for (int i = 0; i < nrOfDOFsProcessed; i++)
{
A(i, i) = 1;
A(i, i+nrOfDOFsProcessed) = periodInSeconds;
A(i, i+2*nrOfDOFsProcessed) = 0.5*periodInSeconds*periodInSeconds;

A(i+nrOfDOFsProcessed, i+nrOfDOFsProcessed) = 1;
A(i+nrOfDOFsProcessed, i+2*nrOfDOFsProcessed) = periodInSeconds;

A(i+2*nrOfDOFsProcessed, i+2*nrOfDOFsProcessed) = 1;
}

iDynTree::MatrixDynSize C(nrOfDOFsProcessed, 3*nrOfDOFsProcessed);
iDynTree::toEigen(C).setIdentity();

iDynTree::MatrixDynSize Q(3*nrOfDOFsProcessed,3*nrOfDOFsProcessed);
if(!this->initCovarianceMatrix(config, "processNoiseCovariance", Q))
{
yError() << " wholeBodyDynamics : failed to set process noise covariance matrix. ";
return false;
}

iDynTree::MatrixDynSize R(nrOfDOFsProcessed,nrOfDOFsProcessed);
if(!this->initCovarianceMatrix(config, "measurementNoiseCovariance", R))
{
yError() << " wholeBodyDynamics : failed to set measurement noise covariance matrix. ";
return false;
}

iDynTree::MatrixDynSize P0(3*nrOfDOFsProcessed,3*nrOfDOFsProcessed);
if(!this->initCovarianceMatrix(config, "stateCovariance", P0))
{
yError() << " wholeBodyDynamics : failed to set initial state covariance matrix. ";
return false;
}

if (!kf->constructKalmanFilter(A, C))
{
yError() << " wholeBodyDynamics : kalman filter cannot be constructed. ";
return false;
}

bool ok = kf->kfSetSystemNoiseCovariance(Q) && kf->kfSetMeasurementNoiseCovariance(R) && kf->kfSetStateCovariance(P0);
if(!ok)
{
yError() << " wholeBodyDynamics : unable to initialize the kf covariances. ";
return false;
}

iDynTree::VectorDynSize x0;
x0.resize(3*nrOfDOFsProcessed);
x0.zero();
if (!kf->kfSetInitialState(x0))
{
yError() << " wholeBodyDynamics : Impossible to set initial state. ";
return false;
}

if (!kf->kfInit())
{
yError() << " wholeBodyDynamics : Impossible to initialize kalman fitler. ";
return false;
}

return true;
}

void wholeBodyDynamicsDeviceFilters::init(int nrOfFTSensors,
double initialCutOffForFTInHz,
double initialCutOffForIMUInHz,
Expand Down Expand Up @@ -3394,6 +3624,11 @@ void wholeBodyDynamicsDeviceFilters::fini()
delete jntAccFilter;
jntAccFilter = 0;
}

if( jntVelAccKFFilter )
{
jntVelAccKFFilter.reset(nullptr);
}
}

wholeBodyDynamicsDeviceFilters::~wholeBodyDynamicsDeviceFilters()
Expand Down
Loading

0 comments on commit bf7d5ff

Please sign in to comment.