diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 0f788d42620..61c97ed3b12 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -333,10 +333,14 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow) { - std::shared_ptr shape = BulletCollisionShapeCache::get(geom); - if (shape != nullptr) - return shape; - + // std::shared_ptr shape = BulletCollisionShapeCache::get(geom); + // if (shape != nullptr) + // { + // // sleep(1000); + // return shape; + // } + + std::shared_ptr shape; switch (geom->getType()) { case tesseract_geometry::GeometryType::BOX: @@ -401,7 +405,7 @@ std::shared_ptr createShapePrimitive(const CollisionShapeConst // LCOV_EXCL_STOP } - BulletCollisionShapeCache::insert(geom, shape); + // BulletCollisionShapeCache::insert(geom, shape); return shape; } diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index 7211187bd94..1a3ed6f6d4d 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -207,13 +207,13 @@ CollisionGeometryPtr createShapePrimitiveHelper(const CollisionShapeConstPtr& ge CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) { - CollisionGeometryPtr shape = FCLCollisionGeometryCache::get(geom); - if (shape != nullptr) - return shape; + // CollisionGeometryPtr shape = FCLCollisionGeometryCache::get(geom); + // if (shape != nullptr) + // return shape; - shape = createShapePrimitiveHelper(geom); - FCLCollisionGeometryCache::insert(geom, shape); - return shape; + // shape = createShapePrimitiveHelper(geom); + // FCLCollisionGeometryCache::insert(geom, shape); + return createShapePrimitiveHelper(geom); } bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data)