Merge pull request #243 from LoreMoretti/bug_fix

Fix memory allocation bug in ForwardKinematics and RelativeTransform blocks
robotology · Feb 6, 2024 · 19b7408 · 19b7408
2 parents a30a6fc + 8425b7c
commit 19b7408
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 5 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [unreleased]
 
+## [5.6.1] - 2023-02-06
+
+## Fixed
+
+- Fix memory allocation bug in ForwardKinematics and RelativeTransform blocks (https://github.com/robotology/wb-toolbox/pull/243).
+
 ## [5.6.0] - 2022-10-21
 
 ### Changed

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -3,7 +3,7 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 cmake_minimum_required(VERSION 3.5)
-project(WB-Toolbox LANGUAGES CXX VERSION 5.6.0)
+project(WB-Toolbox LANGUAGES CXX VERSION 5.6.1)
 
 if(WBT_BUILD_DOCS)
     add_subdirectory(doc)

diff --git a/toolbox/library/src/ForwardKinematics.cpp b/toolbox/library/src/ForwardKinematics.cpp
@@ -227,8 +227,8 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo)
     }
 
     // Allocate objects for row-major -> col-major conversion
-    Map<const Matrix4diDynTree> world_H_frame_RowMajor =
-        toEigen(world_H_frame.asHomogeneousTransform());
+    const iDynTree::Matrix4x4 world_H_frame_asHomogeneous = world_H_frame.asHomogeneousTransform();
+    Map<const Matrix4diDynTree> world_H_frame_RowMajor = toEigen(world_H_frame_asHomogeneous);
     Map<Matrix4dSimulink> world_H_frame_ColMajor(output->getBuffer<double>(), 4, 4);
 
     // Forward the buffer to Simulink transforming it to ColMajor

diff --git a/toolbox/library/src/RelativeTransform.cpp b/toolbox/library/src/RelativeTransform.cpp
@@ -235,8 +235,9 @@ bool RelativeTransform::output(const BlockInformation* blockInfo)
     }
 
     // Allocate objects for row-major -> col-major conversion
-    Map<const Matrix4diDynTree> frame1_H_frame2_RowMajor =
-        toEigen(frame1_H_frame2.asHomogeneousTransform());
+    const iDynTree::Matrix4x4 frame1_H_frame2_asHomogeneous =
+        frame1_H_frame2.asHomogeneousTransform();
+    Map<const Matrix4diDynTree> frame1_H_frame2_RowMajor = toEigen(frame1_H_frame2_asHomogeneous);
     Map<Matrix4dSimulink> frame1_H_frame2_ColMajor(output->getBuffer<double>(), 4, 4);
 
     // Forward the buffer to Simulink transforming it to ColMajor