Merge pull request #698 from prashanthr05/fix/LO

Fix bug in init() of SimpleLeggedOdometry
robotology · Jun 15, 2020 · d805354 · d805354
2 parents ba43b00 + 89dff6b
commit d805354
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,6 +7,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Fixed
+- Fix bug in init() of SimpleLeggedOdometry that used an incorrect initial world frame location if used with an additional frame of a link (https://github.com/robotology/idyntree/pull/698)
+
 ## [1.1.0] - 2020-06-08
 
 ### Added

diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp
@@ -68,7 +68,7 @@ bool SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex,
     m_fixedLinkIndex = m_model.getFrameLink(initialFixedFrameIndex);
 
     Transform world_H_initialFixedFrame = initialFixedFrame_H_world.inverse();
-    Transform initalFixedFrame_H_fixedLink =  m_model.getFrameTransform(m_fixedLinkIndex).inverse();
+    Transform initalFixedFrame_H_fixedLink =  m_model.getFrameTransform(initialFixedFrameIndex).inverse();
 
     m_world_H_fixedLink = world_H_initialFixedFrame*initalFixedFrame_H_fixedLink;