diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 8d9a4db462784a003d3dead33a62a40a3716502f..585c023353b17d4f6a00f49dde2ac4660ad9f14d 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -236,6 +236,9 @@ SET(SOURCES
IK/CompositeDiffIK/CompositeDiffIK.cpp
IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
+
+ IK/platform/OmniWheelPlatformKinematics.cpp
+ IK/platform/MecanumPlatformKinematics.cpp
Import/RobotImporterFactory.cpp
Import/SimoxXMLFactory.cpp
@@ -343,6 +346,7 @@ SET(SOURCES
Workspace/Manipulability.cpp
Workspace/Reachability.cpp
+ Workspace/NaturalPosture.cpp
Workspace/WorkspaceDataArray.cpp
Workspace/WorkspaceGrid.cpp
Workspace/WorkspaceRepresentation.cpp
@@ -451,6 +455,9 @@ SET(INCLUDES
IK/CompositeDiffIK/Soechting.h
IK/CompositeDiffIK/SoechtingNullspaceGradient.h
+ IK/platform/OmniWheelPlatformKinematics.h
+ IK/platform/MecanumPlatformKinematics.h
+
Import/RobotImporterFactory.h
Import/SimoxXMLFactory.h
Import/MeshImport/AssimpReader.h
@@ -584,6 +591,7 @@ SET(INCLUDES
Workspace/Manipulability.h
Workspace/Reachability.h
+ Workspace/NaturalPosture.h
Workspace/VoxelTree6D.hpp
Workspace/VoxelTree6DElement.hpp
Workspace/VoxelTreeND.hpp
diff --git a/VirtualRobot/IK/platform/CMakeLists.txt b/VirtualRobot/IK/platform/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2e1536f3a8445ba90d75ff74b74879ac4fa126c2
--- /dev/null
+++ b/VirtualRobot/IK/platform/CMakeLists.txt
@@ -0,0 +1,13 @@
+armarx_component_set_name("Armar6MecanumPlatform")
+armarx_set_target("Library: Armar6MecanumPlatform")
+
+set(LIB_NAME Armar6MecanumPlatform)
+
+set(LIBS ArmarXCore SimoxUtility)
+
+set(LIB_FILES Armar6MecanumPlatform.cpp)
+set(LIB_HEADERS Armar6MecanumPlatform.h)
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp b/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..a5104d414cf2d82d7cff78e22fffed81afe21728
--- /dev/null
+++ b/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp
@@ -0,0 +1,68 @@
+
+
+#include "MecanumPlatformKinematics.h"
+
+namespace VirtualRobot
+{
+ Eigen::Matrix<float, 4, 3> MecanumPlatformKinematicsParams::J_inv() const
+ {
+ const float k = l1 + l2;
+
+ Eigen::Matrix<float, 4, 3> m;
+ // clang-format off
+ m << 1, 1, -k,
+ -1, 1, k,
+ -1, 1, -k,
+ 1, 1, k;
+ // clang-format on
+
+ m *= 1 / R;
+
+ return m;
+ }
+
+ Eigen::Matrix<float, 3, 4> MecanumPlatformKinematicsParams::J() const
+ {
+ const float k = 1 / (l1 + l2);
+
+ Eigen::Matrix<float, 3, 4> m;
+
+ // according to the paper
+ // m << -1, 1, 1, -1,
+ // 1, 1, 1, 1,
+ // -k, k, -k, k;
+
+ // "ours"
+ // clang-format off
+ m << 1, -1, -1, 1,
+ 1, 1, 1, 1,
+ -k, k, -k, k;
+ // clang-format on
+
+ m *= R / 4;
+
+ return m;
+ }
+
+ MecanumPlatformKinematics::MecanumPlatformKinematics(const Params& params) : params(params)
+ {
+ }
+
+ MecanumPlatformKinematics::WheelVelocities
+ MecanumPlatformKinematics::calcWheelVelocity(const CartesianVelocity& v)
+ {
+ return J_inv * v;
+ }
+
+ MecanumPlatformKinematics::CartesianVelocity
+ MecanumPlatformKinematics::calcCartesianVelocity(const WheelVelocities& w)
+ {
+ return J * w;
+ }
+
+ const MecanumPlatformKinematics::Params& MecanumPlatformKinematics::getParams() const
+ {
+ return params;
+ }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/platform/MecanumPlatformKinematics.h b/VirtualRobot/IK/platform/MecanumPlatformKinematics.h
new file mode 100755
index 0000000000000000000000000000000000000000..f7b4f5d20a89e3531546ea7ab454a63df5b198fd
--- /dev/null
+++ b/VirtualRobot/IK/platform/MecanumPlatformKinematics.h
@@ -0,0 +1,107 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Armar6RT::ArmarXObjects::Armar6MecanumPlatform
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2017, 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+namespace VirtualRobot
+{
+
+ /**
+ * @brief The parameters to define the forward and inverse model of the mecanum model
+ *
+ * See:
+ *
+ * Doroftei, Ioan, Victor Grosu, and Veaceslav Spinu. "Omnidirectional Mobile Robot - Design and Implementation."
+ * Bioinspiration and Robotics Walking and Climbing Robots. IntechOpen, 2007. https://doi.org/10.5772/5518.
+ *
+ * The variable names are consistent with the paper.
+ *
+ * Note:
+ * In the paper, the x-axis is pointing forwards. Here, y is pointing forwards.
+ */
+ struct MecanumPlatformKinematicsParams
+ {
+ //! gauge
+ float l1;
+
+ //! wheelbase
+ float l2;
+
+ //! wheel radius
+ float R;
+
+ // See Doroftei et al., formula 9
+ Eigen::Matrix<float, 3, 4> J() const;
+
+ // See Doroftei et al., formula 8
+ Eigen::Matrix<float, 4, 3> J_inv() const;
+ };
+
+ /**
+ * @brief The kinematic model of the mecanum platform model
+ *
+ * See:
+ *
+ * Doroftei, Ioan, Victor Grosu, and Veaceslav Spinu. "Omnidirectional Mobile Robot - Design and Implementation."
+ * Bioinspiration and Robotics Walking and Climbing Robots. IntechOpen, 2007. https://doi.org/10.5772/5518.
+ *
+ * The variable names are consistent with the paper.
+ *
+ *
+ * Wheel definition:
+ *
+ * - wheel 0: left front
+ * - wheel 1: right front
+ * - wheel 2: rear left
+ * - wheel 3: rear right
+ */
+ class MecanumPlatformKinematics
+ {
+ public:
+ using Params = MecanumPlatformKinematicsParams;
+
+ using WheelVelocities = Eigen::Vector4f;
+
+ //! \f$ [\dot{x}, \dot{y}, \dot{yaw}] \f$
+ using CartesianVelocity = Eigen::Vector3f;
+
+ MecanumPlatformKinematics(const Params& params);
+
+ //! inverse model
+ WheelVelocities calcWheelVelocity(const CartesianVelocity& v);
+
+ //! forward model
+ CartesianVelocity calcCartesianVelocity(const WheelVelocities& w);
+
+ const Params& getParams() const;
+
+ private:
+ const Params params;
+
+ const Eigen::Matrix<float, 3, 4> J;
+ const Eigen::Matrix<float, 4, 3> J_inv;
+ };
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..5fb5760b32c6028443a4b56e42430a0aa5e08d96
--- /dev/null
+++ b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp
@@ -0,0 +1,70 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "OmniWheelPlatformKinematics.h"
+
+namespace VirtualRobot
+{
+ // OmniWheelPlatformKinematicsParams
+
+ Eigen::Matrix3f OmniWheelPlatformKinematicsParams::B() const
+ {
+ Eigen::Matrix3f b;
+
+ // clang-format off
+ b << 1.F, -std::sin(delta), -std::sin(delta),
+ 0.F, std::cos(delta), -std::cos(delta),
+ L, L, L;
+ // clang-format on
+
+ return b;
+ }
+
+ Eigen::Matrix3f OmniWheelPlatformKinematicsParams::C() const
+ {
+ return B().transpose().inverse() * R / n;
+ }
+
+ // OmniWheelPlatformKinematics
+
+ OmniWheelPlatformKinematics::OmniWheelPlatformKinematics(const Params& params) :
+ params(params), C(params.C()), C_inv(C.inverse())
+ {
+ }
+
+ OmniWheelPlatformKinematics::WheelVelocities
+ OmniWheelPlatformKinematics::calcWheelVelocity(const CartesianVelocity& v)
+ {
+ return C * v;
+ }
+
+ OmniWheelPlatformKinematics::CartesianVelocity
+ OmniWheelPlatformKinematics::calcCartesianVelocity(const WheelVelocities& w)
+ {
+ return C_inv * w;
+ }
+
+ const OmniWheelPlatformKinematics::Params& OmniWheelPlatformKinematics::getParams() const
+ {
+ return params;
+ }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h
new file mode 100755
index 0000000000000000000000000000000000000000..e816d016ae04d79c252541c2242b3359365d138a
--- /dev/null
+++ b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+namespace VirtualRobot
+{
+
+ /**
+ * @brief The parameters to define the forward and inverse model of the omni wheel model
+ *
+ * See:
+ *
+ * Liu, Yong, Xiaofei Wu, J. Jim Zhu, and Jae Lew. “Omni-Directional Mobile Robot Controller Design by Trajectory Linearization.â€
+ * In Proceedings of the 2003 American Control Conference, 2003., 4:3423–28 vol.4, 2003. https://doi.org/10.1109/ACC.2003.1244061.
+ *
+ * The variable names are consistent with the paper.
+ *
+ */
+ struct OmniWheelPlatformKinematicsParams
+ {
+ //! radius of the body
+ float L;
+
+ //! wheel radius
+ float R;
+
+ //! angular position of the first wheel
+ float delta;
+
+ //! gear ratio
+ float n;
+
+ // see Liu et al., formula 2.2.2
+ Eigen::Matrix3f B() const;
+
+ // forward model, similar to Jacobian matrix, see Liu et al., formula 2.2.2
+ Eigen::Matrix3f C() const;
+ };
+
+ /**
+ * @brief The kinematic model of the omni wheel platform model
+ *
+ * See:
+ *
+ * Liu, Yong, Xiaofei Wu, J. Jim Zhu, and Jae Lew. “Omni-Directional Mobile Robot Controller Design by Trajectory Linearization.â€
+ * In Proceedings of the 2003 American Control Conference, 2003., 4:3423–28 vol.4, 2003. https://doi.org/10.1109/ACC.2003.1244061.
+ *
+ */
+ class OmniWheelPlatformKinematics
+ {
+ public:
+ using Params = OmniWheelPlatformKinematicsParams;
+
+ //! [w_m1, w_m2, w_m3]
+ using WheelVelocities = Eigen::Vector3f;
+
+ //! [u, v, r]
+ using CartesianVelocity = Eigen::Vector3f;
+
+ OmniWheelPlatformKinematics(const Params& info);
+
+ //! inverse model
+ WheelVelocities calcWheelVelocity(const CartesianVelocity& v);
+
+ //! forward model
+ CartesianVelocity calcCartesianVelocity(const WheelVelocities& w);
+
+ const Params& getParams() const;
+
+ private:
+ const Params params;
+
+ const Eigen::Matrix3f C;
+ const Eigen::Matrix3f C_inv;
+ };
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/platform/test/Armar6MecanumPlatformTest.cpp b/VirtualRobot/IK/platform/test/Armar6MecanumPlatformTest.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..0f0e7f67114dd0810c06086ada1724c8f51bdce3
--- /dev/null
+++ b/VirtualRobot/IK/platform/test/Armar6MecanumPlatformTest.cpp
@@ -0,0 +1,92 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Armar6RT::ArmarXObjects::Armar6MecanumPlatform
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Armar6RT::ArmarXLibraries::Armar6MecanumPlatform
+
+#define ARMARX_BOOST_TEST
+
+#include <Armar6RT/Test.h>
+#include "../Armar6MecanumPlatform.h"
+
+#include <iostream>
+
+#include <Armar6RT/libraries/Armar6MecanumPlatform/Armar6MecanumPlatform.h>
+
+#include <Eigen/src/Core/IO.h>
+
+using namespace armarx;
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ Eigen::IOFormat vecfmt(5, 0, "", ", ", "", "", "(", ")");
+ Armar6MecanumPlatform p;
+ std::cout << "1 0 0: " << p.calcWheelVelocity(1, 0, 0).format(vecfmt) << std::endl;
+ std::cout << "0 1 0: " << p.calcWheelVelocity(0, 1, 0).format(vecfmt) << std::endl;
+ std::cout << "0 0 1: " << p.calcWheelVelocity(0, 0, 1).format(vecfmt) << std::endl;
+ BOOST_CHECK_EQUAL(true, true);
+}
+
+
+
+BOOST_AUTO_TEST_CASE(testInv)
+{
+ Armar6MecanumPlatform p;
+
+ float vx = 100.0;
+ float vy = 200.0;
+ float vAngle = 0.1f;
+
+ Eigen::Vector4f wheelVelocities = p.calcWheelVelocity(vx, vy, vAngle);
+ Eigen::Vector3f velocities = p.calcCartesianVelocity(wheelVelocities);
+
+ BOOST_TEST(vx == velocities(0), boost::test_tools::tolerance(0.1));
+ BOOST_TEST(vy == velocities(1), boost::test_tools::tolerance(0.1));
+ BOOST_TEST(vAngle == velocities(2), boost::test_tools::tolerance(0.001));
+}
+
+
+
+BOOST_AUTO_TEST_CASE(testMoveForwardCartesianVel)
+{
+ Armar6MecanumPlatform p;
+
+ Eigen::Vector4f wheelVelocities = Eigen::Vector4f::Ones();
+ Eigen::Vector3f velocities = p.calcCartesianVelocity(wheelVelocities);
+
+ BOOST_TEST(0.0 == velocities(0), boost::test_tools::tolerance(0.1));
+ BOOST_TEST(0.0 < velocities(1));
+ BOOST_TEST(0.0 == velocities(2), boost::test_tools::tolerance(0.001));
+}
+
+
+BOOST_AUTO_TEST_CASE(testMoveForwardWheelVel)
+{
+ Armar6MecanumPlatform p;
+
+ Eigen::Vector4f wheelVelocities = p.calcWheelVelocity(0.0, 100.0, 0.0);
+
+ BOOST_TEST(0 < wheelVelocities(0));
+ BOOST_TEST(wheelVelocities(0) == wheelVelocities(1), boost::test_tools::tolerance(0.1));
+ BOOST_TEST(wheelVelocities(0) == wheelVelocities(2), boost::test_tools::tolerance(0.1));
+ BOOST_TEST(wheelVelocities(0) == wheelVelocities(3), boost::test_tools::tolerance(0.1));
+}
+
diff --git a/VirtualRobot/IK/platform/test/CMakeLists.txt b/VirtualRobot/IK/platform/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..61c8ca6a292fcfa12c24ce39aba362b078402f41
--- /dev/null
+++ b/VirtualRobot/IK/platform/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore Armar6MecanumPlatform)
+
+armarx_add_test(Armar6MecanumPlatformTest Armar6MecanumPlatformTest.cpp "${LIBS}")
\ No newline at end of file