diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h
index 8803c4a6b94..5e8b6d3b403 100644
--- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h
+++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h
@@ -116,7 +116,7 @@ class CollisionObjectWrapper
     for (unsigned i = 0; i < collision_objects_.size(); ++i)
     {
       CollisionObjectPtr& co = collision_objects_[i];
-      co->setTransform(pose * shape_poses_[i]);
+      co->setTransform(pose * collision_poses_[i]);
       co->updateAABB();  // This a tesseract function that updates abb to take into account contact distance
     }
   }
@@ -141,6 +141,7 @@ class CollisionObjectWrapper
     clone_cow->type_id_ = type_id_;
     clone_cow->shapes_ = shapes_;
     clone_cow->shape_poses_ = shape_poses_;
+    clone_cow->collision_poses_ = collision_poses_;
     clone_cow->collision_geometries_ = collision_geometries_;
 
     clone_cow->collision_objects_.reserve(collision_objects_.size());
@@ -176,6 +177,7 @@ class CollisionObjectWrapper
   Eigen::Isometry3d world_pose_{ Eigen::Isometry3d::Identity() }; /**< @brief Collision Object World Transformation */
   CollisionShapesConst shapes_;
   tesseract_common::VectorIsometry3d shape_poses_;
+  tesseract_common::VectorIsometry3d collision_poses_;
   std::vector<CollisionGeometryPtr> collision_geometries_;
   std::vector<CollisionObjectPtr> collision_objects_;
   /**
diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp
index b2cac88505d..61e99aa46de 100644
--- a/tesseract_collision/fcl/src/fcl_utils.cpp
+++ b/tesseract_collision/fcl/src/fcl_utils.cpp
@@ -349,6 +349,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
   m_collisionFilterGroup = CollisionFilterGroups::KinematicFilter;
   m_collisionFilterMask = CollisionFilterGroups::StaticFilter | CollisionFilterGroups::KinematicFilter;
 
+  collision_poses_.reserve(shapes_.size());
   collision_geometries_.reserve(shapes_.size());
   collision_objects_.reserve(shapes_.size());
   collision_objects_raw_.reserve(shapes_.size());
@@ -362,6 +363,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
         CollisionGeometryPtr subshape = createShapePrimitive(mesh);
         if (subshape != nullptr)
         {
+          collision_poses_.push_back(shape_poses_[i]);
           collision_geometries_.push_back(subshape);
           auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
           co->setUserData(this);
@@ -377,6 +379,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
       CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]);
       if (subshape != nullptr)
       {
+        collision_poses_.push_back(shape_poses_[i]);
         collision_geometries_.push_back(subshape);
         auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
         co->setUserData(this);