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
robotology · Aug 7, 2017 · af1f1d8 · af1f1d8
1 parent 5ad56eb
commit af1f1d8
Showing 1 changed file with 6 additions and 1 deletion.
diff --git 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);