diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index ecdd9f825e6442376cd19cd0c3a61a2ac590f51b..057440c94c97f8428c191c2d9c7a83edc95cbf51 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -38,3 +38,4 @@ add_subdirectory(skills)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
+add_subdirectory(obstacle_avoidance)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dfd09c4a46abb303d29f4a0e5385fb6aa0a16b7b
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
@@ -0,0 +1,28 @@
+set(LIB_NAME obstacle_avoidance)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::armem_vision
+ RobotAPI::ArmarXObjects
+ aroncommon
+ # System / External
+# Eigen3::Eigen
+ HEADERS
+ CollisionModelHelper.h
+ SOURCES
+ CollisionModelHelper.cpp
+)
+
+add_library(
+ "RobotAPI::${LIB_NAME}"
+ ALIAS
+ ${LIB_NAME}
+)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8be378fc4908f29400a3ce9c81e00be5cae3c66
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -0,0 +1,154 @@
+/*
+ * 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 RobotAPI
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CollisionModelHelper.h"
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::obstacle_avoidance
+{
+
+ VirtualRobot::ManipulationObjectPtr
+ CollisionModelHelper::asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ const ObjectFinder finder;
+
+ const VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ const ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params)
+ {
+ const OccupancyGridHelper ocHelper(occupancyGrid, params);
+ const auto obstacles = ocHelper.obstacles();
+
+ const float boxSize = occupancyGrid.resolution;
+ const float resolution = occupancyGrid.resolution;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+
+ ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
+ << "Only occupancy grid in global frame supported.";
+
+ VirtualRobot::CoinVisualizationFactory factory;
+
+ const auto& world_T_map = occupancyGrid.pose;
+
+ for (int x = 0; x < obstacles.rows(); x++)
+ {
+ for (int y = 0; y < obstacles.cols(); y++)
+ {
+ if (obstacles(x, y))
+ {
+ const Eigen::Vector3f pos{
+ static_cast<float>(x) * resolution, static_cast<float>(y) * resolution, 0};
+
+ // FIXME: change to Isometry3f
+ Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
+ map_T_obj.translation() = pos;
+
+ Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
+
+ // ARMARX_INFO << world_T_obj.translation();
+
+ auto cube = factory.createBox(boxSize, boxSize, boxSize);
+
+ const VirtualRobot::CollisionModelPtr collisionModel(
+ new VirtualRobot::CollisionModel(cube));
+
+ const VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
+ "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
+ sceneObject->setGlobalPose(world_T_obj.matrix());
+
+ sceneObjects->addSceneObject(sceneObject);
+ }
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ CollisionModelHelper::CollisionModelHelper(const objpose::ObjectPoseClient& client) :
+ objectPoseClient_(client)
+ {
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::fetchSceneObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asSceneObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::fetchManipulationObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asManipulationObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ManipulationObjectSet set;
+
+ for (const auto& pose : objectPoses)
+ {
+ set.emplace_back(*asManipulationObject(pose));
+ }
+
+ return std::make_shared<ManipulationObjectSet>(set);
+ }
+
+} // namespace armarx::obstacle_avoidance
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca677b5113ffda4feca14cd89e17ff2e6419eaf9
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
@@ -0,0 +1,57 @@
+/*
+ * 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 RobotAPI
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::obstacle_avoidance
+{
+
+ class CollisionModelHelper
+ {
+ public:
+ using ManipulationObjectSet = std::vector<VirtualRobot::ManipulationObject>;
+ using ManipulationObjectSetPtr = std::shared_ptr<ManipulationObjectSet>;
+
+ static VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose);
+ static ManipulationObjectSetPtr
+ asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params);
+
+ CollisionModelHelper(const objpose::ObjectPoseClient& client);
+ VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
+ ManipulationObjectSetPtr fetchManipulationObjects();
+ private:
+ objpose::ObjectPoseClient objectPoseClient_;
+ };
+} // namespace armarx::obstacle_avoidance