diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 428b5e2cdd..c117f4b3fc 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -157,6 +157,9 @@ class World * verified by comparing pointers. Returns true on success. */ bool moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose); + /** \brief Move all shapes in an object according to the given transform specified in world frame */ + bool moveObject(const std::string& id, const Eigen::Affine3d& transform); + /** \brief Remove shape from object. * Shape equality is verified by comparing pointers. Ownership of the * object is renounced (i.e. object is deleted if no external references diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index b280e58e79..454b1b99cc 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -155,6 +155,20 @@ bool World::moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr return false; } +bool World::moveObject(const std::string& id, const Eigen::Affine3d& transform) +{ + auto it = objects_.find(id); + if (it == objects_.end()) + return false; + ensureUnique(it->second); + for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i) + { + it->second->shape_poses_[i] = transform * it->second->shape_poses_[i]; + } + notify(it->second, MOVE_SHAPE); + return true; +} + bool World::removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape) { auto it = objects_.find(id); @@ -247,4 +261,4 @@ void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Actio } } -} // end of namespace collision_detection \ No newline at end of file +} // end of namespace collision_detection