diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index cbfb380d13923f7fb5eefa67693a6ff8646744e3..82f363703cd28aec99ae2395035bbb780f38d858 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -354,18 +354,17 @@ armarx::ObstacleAwarePlatformUnit::get_velocities()
 
     // Acquire control_data mutex to ensure data remains consistent.
     std::scoped_lock l{m_control_data.mutex};
-    ARMARX_LOG << __LINE__;
     // Update agent and get target velocities.
     update_agent_dependent_values();
-    ARMARX_LOG << __LINE__;
     const Eigen::Vector2f target_vel = get_target_velocity();
-    ARMARX_LOG << __LINE__;
     const float target_rot_vel = get_target_rotational_velocity();
-    ARMARX_LOG << __LINE__;
+
+    const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
     // Apply obstacle avoidance and apply post processing to get the modulated velocity.
-    const Eigen::Vector2f modulated_vel = [this, &target_vel]
+    const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
     {
-        const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, m_control_data.target_pos);
+        const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, extents, m_control_data.agent_ori, m_control_data.target_pos);
 
         ARMARX_DEBUG << "Distance to obstacle: " << distance;