diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
index 9dd4a0c1..cf20fbe1 100644
--- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
+++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
@@ -126,13 +126,18 @@ inline visualization_msgs::Marker MarkerShape<T>::getMarker()
 template <typename T>
 FCL_CollisionObject MarkerShape<T>::getCollisionObject() const
 {
+    // FIXME
+    // fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
+    //                                   this->marker_.pose.orientation.x,
+    //                                   this->marker_.pose.orientation.y,
+    //                                   this->marker_.pose.orientation.z),
+    //                     FCL_Vec3(this->marker_.pose.position.x,
+    //                              this->marker_.pose.position.y,
+    //                              this->marker_.pose.position.z));
     fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
                                       this->marker_.pose.orientation.x,
                                       this->marker_.pose.orientation.y,
-                                      this->marker_.pose.orientation.z),
-                        FCL_Vec3(this->marker_.pose.position.x,
-                                 this->marker_.pose.position.y,
-                                 this->marker_.pose.position.z));
+                                      this->marker_.pose.orientation.z));
 
     FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
     return cobj;
diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp
index 9a238131..ea91f423 100644
--- a/cob_obstacle_distance/src/distance_manager.cpp
+++ b/cob_obstacle_distance/src/distance_manager.cpp
@@ -273,7 +273,9 @@ void DistanceManager::calculate()
                 FCL_CollisionObject collision_obj = obstacle->getCollisionObject();
                 FCL_DistanceResult dist_result;
                 FCL_DistanceRequest dist_request(true, 5.0, 0.01);
-                fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result);
+                //FIXME
+                // double dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result);
+                double dist = 0;
 
 
                 Eigen::Vector3d abs_obst_vector(dist_result.nearest_points[1][VEC_X],
diff --git a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
index 5f2f6b0a..1503ef1c 100644
--- a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
+++ b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
@@ -165,13 +165,18 @@ inline visualization_msgs::Marker MarkerShape<BVH_RSS_t>::getMarker()
 
 FCL_CollisionObject MarkerShape<BVH_RSS_t>::getCollisionObject() const
 {
+    // FIXME
+    // fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
+    //                                   this->marker_.pose.orientation.x,
+    //                                   this->marker_.pose.orientation.y,
+    //                                   this->marker_.pose.orientation.z),
+    //                    FCL_Vec3(this->marker_.pose.position.x,
+    //                             this->marker_.pose.position.y,
+    //                             this->marker_.pose.position.z));
     fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
                                       this->marker_.pose.orientation.x,
                                       this->marker_.pose.orientation.y,
-                                      this->marker_.pose.orientation.z),
-                       FCL_Vec3(this->marker_.pose.position.x,
-                                this->marker_.pose.position.y,
-                                this->marker_.pose.position.z));
+                                      this->marker_.pose.orientation.z));
     FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
     return cobj;
 }