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

ExternalWrenchesEstimation: if there are no unknowns do not do the pseudoinverse #443

Merged
merged 3 commits into from
May 24, 2018
Merged

ExternalWrenchesEstimation: if there are no unknowns do not do the pseudoinverse #443

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
01c7c71
ExternalWrenchesEstimation: if there are no unknowns do not do the ps…
traversaro May 24, 2018
01f980d
Update release notes
traversaro May 24, 2018
f6d03c5
Update ExternalWrenchesEstimation.cpp
traversaro May 24, 2018
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
1 change: 1 addition & 0 deletions doc/releases/v0_8_2.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,4 @@ iDynTree 0.8.2 Release Notes

Bug Fixes
---------
* In the classes devoted to the external force-torque estimation, since https://github.com/robotology/idyntree/pull/343 it is possible to have external forces that are completly estimated from external sensors such as skin sensors. If in a given submodels there are no unknown due to this, the pseudoinverse should be skipped ( https://github.com/robotology/idyntree/pull/443 ) .
34 changes: 21 additions & 13 deletions src/estimation/src/ExternalWrenchesEstimation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -601,13 +601,17 @@ bool estimateExternalWrenchesWithoutInternalFT(const Model& model,
// Now we compute the A matrix
computeMatrixOfEstimationEquationAndExtWrenchKnownTerms(model,traversal,unknownWrenches,jointPos,subModelIndex,bufs);

// Now we compute the pseudo inverse
pseudoInverse(toEigen(bufs.A[subModelIndex]),
toEigen(bufs.pinvA[subModelIndex]),
tol);

// Now we compute the unknowns
toEigen(bufs.x[subModelIndex]) = toEigen(bufs.pinvA[subModelIndex])*toEigen(bufs.b[subModelIndex]);
// If A has no unkowns then pseudoInverse can not be computed
// In that case, we do not compute the x vector because it will have zero elements
if (bufs.A[subModelIndex].rows() > 0 && bufs.A[subModelIndex].cols() > 0) {
// Now we compute the pseudo inverse
pseudoInverse(toEigen(bufs.A[subModelIndex]),
toEigen(bufs.pinvA[subModelIndex]),
tol);

// Now we compute the unknowns
toEigen(bufs.x[subModelIndex]) = toEigen(bufs.pinvA[subModelIndex])*toEigen(bufs.b[subModelIndex]);
}

// We copy the estimated unknowns in the outputContactWrenches
// Note that the logic of conversion between input/output contacts should be
Expand Down Expand Up @@ -653,13 +657,17 @@ bool estimateExternalWrenches(const Model& model,
// Now we compute the A matrix
computeMatrixOfEstimationEquationAndExtWrenchKnownTerms(model,subModelTraversal,unknownWrenches,jointPos,sm,bufs);

// Now we compute the pseudo inverse
pseudoInverse(toEigen(bufs.A[sm]),
toEigen(bufs.pinvA[sm]),
tol);
// If A has no unkowns then pseudoInverse can not be computed
// In that case, we do not compute the x vector because it will have zero elements
if (bufs.A[sm].rows() > 0 && bufs.A[sm].cols() > 0) {
// Now we compute the pseudo inverse
pseudoInverse(toEigen(bufs.A[sm]),
toEigen(bufs.pinvA[sm]),
tol);

// Now we compute the unknowns
toEigen(bufs.x[sm]) = toEigen(bufs.pinvA[sm])*toEigen(bufs.b[sm]);
// Now we compute the unknowns
toEigen(bufs.x[sm]) = toEigen(bufs.pinvA[sm])*toEigen(bufs.b[sm]);
}

// Check if there are any nan in the estimation results
bool someResultIsNan = false;
Expand Down