From af1f1d831d7e3b458644e47f2415488b9a6935cf Mon Sep 17 00:00:00 2001 From: fjandrad Date: Fri, 4 Aug 2017 18:03:09 +0200 Subject: [PATCH] adding the storage of the already known external contact wrenches so that they are considered when summming the net wrenches. That way external wrenches are also included throughout the proccess of joint torque estimation --- src/estimation/src/ExternalWrenchesEstimation.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/estimation/src/ExternalWrenchesEstimation.cpp b/src/estimation/src/ExternalWrenchesEstimation.cpp index 5ad3a43f9f5..1587588be5a 100644 --- a/src/estimation/src/ExternalWrenchesEstimation.cpp +++ b/src/estimation/src/ExternalWrenchesEstimation.cpp @@ -500,7 +500,7 @@ void computeMatrixOfEstimationEquationAndExtWrenchKnownTerms(const Model& model, break; case NO_UNKNOWNS: { - //AcolsToWrite += 0; + //why is it only using subModelBase_H_link instead of subModelBase_H_contact toEigen(bufs.b[subModelIndex]) += -toEigen(subModelBase_H_link*(unknownWrench.knownWrench)); } break; @@ -564,6 +564,11 @@ void storeResultsOfEstimation(const Traversal& traversal, nextUnknownToRead += 1; break; case NO_UNKNOWNS: + //const Transform & subModelBase_H_link = bufs.subModelBase_H_link(visitedLinkIndex); + //Transform link_H_contact = Transform(Rotation::Identity(),unknownWrench.contactPoint); + //Transform subModelBase_H_contact = subModelBase_H_link*link_H_contact; + //estimatedWrench=subModelBase_H_link*(unknownWrench.knownWrench); + estimatedWrench=unknownWrench.knownWrench; break; default: assert(false);