From 01c7c71d193abcc824dfaa4e037c446726ad2bac Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 24 May 2018 14:32:53 +0200 Subject: [PATCH 1/3] ExternalWrenchesEstimation: if there are no unknowns do not do the pseudoinverse See https://github.com/robotology/idyntree/commit/9517af7a232d0d3819ff24f569f1626319de0801 for the original commit --- .../src/ExternalWrenchesEstimation.cpp | 34 ++++++++++++------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/src/estimation/src/ExternalWrenchesEstimation.cpp b/src/estimation/src/ExternalWrenchesEstimation.cpp index 24cd7df41e0..1b1628f4de3 100644 --- a/src/estimation/src/ExternalWrenchesEstimation.cpp +++ b/src/estimation/src/ExternalWrenchesEstimation.cpp @@ -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[sm].rows() > 0 && bufs.A[sm].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 @@ -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; From 01f980dd62f639e5152d4fe261f820b62d480ebd Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 24 May 2018 14:43:12 +0200 Subject: [PATCH 2/3] Update release notes --- doc/releases/v0_8_2.md | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/releases/v0_8_2.md b/doc/releases/v0_8_2.md index c4c33fbd656..b68e2b0e065 100644 --- a/doc/releases/v0_8_2.md +++ b/doc/releases/v0_8_2.md @@ -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 ) . From f6d03c56e4566bcf40b7b479dd062bd73c5419de Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 24 May 2018 17:16:52 +0200 Subject: [PATCH 3/3] Update ExternalWrenchesEstimation.cpp --- src/estimation/src/ExternalWrenchesEstimation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimation/src/ExternalWrenchesEstimation.cpp b/src/estimation/src/ExternalWrenchesEstimation.cpp index 1b1628f4de3..88f75d8ac74 100644 --- a/src/estimation/src/ExternalWrenchesEstimation.cpp +++ b/src/estimation/src/ExternalWrenchesEstimation.cpp @@ -603,7 +603,7 @@ bool estimateExternalWrenchesWithoutInternalFT(const Model& model, // 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) { + 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]),