diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5fe983f5..6db837cc 100644
--- 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
index bc97d404..79a1e9a6 100644
--- 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
index fcdce0fd..aa9365fb 100644
--- 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
index 0b44c315..e7e2c094 100644
--- 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