From c97572227dac59aa3ddb2e0c8c6f2c5b36912a28 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 20:32:19 +0100 Subject: [PATCH 01/23] Added CollisionTreeMap, Shape storage and closes robot link --- exotica/include/exotica/KinematicTree.h | 11 +++++-- exotica/src/KinematicTree.cpp | 39 +++++++++++++++++++++---- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/exotica/include/exotica/KinematicTree.h b/exotica/include/exotica/KinematicTree.h index dcf8817f59..b3be93ee0f 100644 --- a/exotica/include/exotica/KinematicTree.h +++ b/exotica/include/exotica/KinematicTree.h @@ -85,7 +85,7 @@ namespace exotica { public: KinematicFrameRequest(); - KinematicFrameRequest(std::string frameALinkName, KDL::Frame frameAOffset = KDL::Frame(), std::string frameBLinkName = "", KDL::Frame frameBOffset = KDL::Frame()); + KinematicFrameRequest(std::string frameALinkName, KDL::Frame frameAOffset = KDL::Frame(), std::string frameBLinkName = "", KDL::Frame frameBOffset = KDL::Frame()); std::string FrameALinkName; KDL::Frame FrameAOffset; std::string FrameBLinkName; @@ -104,15 +104,20 @@ namespace exotica { public: KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment); + void updateClosestRobotLink(); int Id; int ControlId; bool IsControlled; std::shared_ptr Parent; std::vector> Children; + std::shared_ptr ClosestRobotLink; KDL::Segment Segment; KDL::Frame Frame; std::vector JointLimits; shapes::ShapeConstPtr Shape; + bool isRobotLink; + private: + void setChildrenClosestRobotLink(std::shared_ptr element); }; struct KinematicFrame @@ -194,7 +199,7 @@ namespace exotica Eigen::VectorXd getControlledState(); std::vector> getTree() {return Tree;} - std::vector> getCollisionTree() {return CollisionTree;} + std::map> getCollisionTreeMap() {return CollisionTreeMap;} std::map> getTreeMap() {return TreeMap;} bool Debug; @@ -218,7 +223,7 @@ namespace exotica std::vector> Tree; std::vector> ModelTree; std::map> TreeMap; - std::vector> CollisionTree; + std::map> CollisionTreeMap; std::shared_ptr Root; std::vector> ControlledJoints; std::map> ControlledJointsMap; diff --git a/exotica/src/KinematicTree.cpp b/exotica/src/KinematicTree.cpp index 26c65d6a99..a408066bc2 100644 --- a/exotica/src/KinematicTree.cpp +++ b/exotica/src/KinematicTree.cpp @@ -46,11 +46,36 @@ namespace exotica { -KinematicElement::KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment) : Parent(parent), Segment(segment), Id(id), IsControlled(false), ControlId(-1), Shape(nullptr) +KinematicElement::KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment) : Parent(parent), Segment(segment), Id(id), IsControlled(false), ControlId(-1), Shape(nullptr), isRobotLink(false), ClosestRobotLink(nullptr) { } +void KinematicElement::setChildrenClosestRobotLink(std::shared_ptr element) +{ + ClosestRobotLink = element; + for(auto& child : Children) + { + child->setChildrenClosestRobotLink(element); + } +} + +void KinematicElement::updateClosestRobotLink() +{ + std::shared_ptr element = Parent; + ClosestRobotLink = nullptr; + while(element && element->Id>0) + { + if(element->isRobotLink) + { + ClosestRobotLink = element; + break; + } + element = element->Parent; + } + setChildrenClosestRobotLink(ClosestRobotLink); +} + KinematicResponse::KinematicResponse() : Flags(KIN_FK) { @@ -202,6 +227,7 @@ void KinematicTree::BuildTree(const KDL::Tree & RobotKinematics) ControlledJoints.resize(NumControlledJoints); for(std::shared_ptr Joint : Tree) { + Joint->isRobotLink = true; Joint->ControlId = IsControlled(Joint); Joint->IsControlled = Joint->ControlId >= 0; if(Joint->IsControlled) ControlledJoints[Joint->ControlId] = Joint; @@ -223,8 +249,7 @@ void KinematicTree::BuildTree(const KDL::Tree & RobotKinematics) ControlledBaseType = ModelBaseType; } } - - + Tree[0]->isRobotLink = false; setJointLimits(); } @@ -244,7 +269,7 @@ void KinematicTree::UpdateModel() void KinematicTree::resetModel() { Tree = ModelTree; - CollisionTree.clear(); + CollisionTreeMap.clear(); UpdateModel(); debugSceneChanged = true; } @@ -266,6 +291,7 @@ void KinematicTree::changeParent(const std::string& name, const std::string& par if(TreeMap.find(parent_name)==TreeMap.end()) throw_pretty("Attempting to attach to unknown frame '"<second; } + if(parent->Shape) throw_pretty("Can't attach object to a collision shape object! ('"<Segment = KDL::Segment(child->Segment.getName(), child->Segment.getJoint(), pose, child->Segment.getInertia()); @@ -279,6 +305,7 @@ void KinematicTree::changeParent(const std::string& name, const std::string& par child->Parent->Children.erase(it); child->Parent = parent; parent->Children.push_back(child); + child->updateClosestRobotLink(); debugSceneChanged = true; } @@ -309,10 +336,11 @@ void KinematicTree::AddElement(const std::string& name, Eigen::Affine3d& transfo if(shape) { NewElement->Shape = shape; - CollisionTree.push_back(NewElement); + CollisionTreeMap[NewElement->Segment.getName()] = NewElement; } Tree.push_back(NewElement); parent_element->Children.push_back(NewElement); + NewElement->updateClosestRobotLink(); debugSceneChanged = true; } @@ -447,6 +475,7 @@ void KinematicTree::publishFrames() shapes::constructMarkerFromShape(Tree[i]->Shape.get(), mrk); mrk.action = visualization_msgs::Marker::ADD; mrk.frame_locked = true; + mrk.id = i; mrk.ns = "CollisionObjects"; mrk.color.r=mrk.color.g=mrk.color.b=0.5; mrk.color.a=1.0; From 43758a39dce44f05fdcea49a81206ebfc7c63bb0 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 20:33:28 +0100 Subject: [PATCH 02/23] Added color tool and removed update counter from ompl solver --- exotations/solvers/ompl_solver/src/OMPLBaseSolver.cpp | 2 -- exotica/include/exotica/Tools.h | 9 +++++++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/exotations/solvers/ompl_solver/src/OMPLBaseSolver.cpp b/exotations/solvers/ompl_solver/src/OMPLBaseSolver.cpp index 1e9a445b6d..e63c693b11 100644 --- a/exotations/solvers/ompl_solver/src/OMPLBaseSolver.cpp +++ b/exotations/solvers/ompl_solver/src/OMPLBaseSolver.cpp @@ -31,8 +31,6 @@ namespace exotica { ompl::base::PlannerData data(ompl_simple_setup_->getSpaceInformation()); ompl_simple_setup_->getPlanner()->getPlannerData(data); - int cnt = prob_->getScene()->getCollisionScene()->stateCheckCnt_; - prob_->getScene()->getCollisionScene()->stateCheckCnt_ = 0; } double OMPLBaseSolver::getPlanningTime() diff --git a/exotica/include/exotica/Tools.h b/exotica/include/exotica/Tools.h index 0cb3aa687d..f549090fee 100644 --- a/exotica/include/exotica/Tools.h +++ b/exotica/include/exotica/Tools.h @@ -59,6 +59,15 @@ namespace exotica * @return Random color */ std_msgs::ColorRGBA randomColor(); + inline std_msgs::ColorRGBA getColor(double r, double g, double b, double a = 1.0) + { + std_msgs::ColorRGBA ret; + ret.r = r; + ret.g = g; + ret.b = b; + ret.a = 1; + return ret; + } /** * @brief loadOBJ Loads mesh data from an OBJ file From 310baa144c47e7e9cc001a932a4dca0802563f70 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 20:39:27 +0100 Subject: [PATCH 03/23] Added simplified allowed collision matrix --- exotica/include/exotica/Scene.h | 42 +++++++++++++++++++++++++++++++++ exotica/src/Scene.cpp | 41 ++++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+) diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 5eaf7f9b22..1337318aa8 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -57,11 +57,53 @@ #include #include +#include +#include + namespace exotica { typedef std::vector geos_ptr; typedef std::vector > fcls_ptr; + class AllowedCollisionMatrix + { + public: + AllowedCollisionMatrix(); + AllowedCollisionMatrix(const AllowedCollisionMatrix& acm); + void clear(); + bool hasEntry(const std::string& name) const; + void setEntry(const std::string& name1, const std::string& name2); + void getAllEntryNames(std::vector& names) const; + bool getAllowedCollision(const std::string& name1, const std::string& name2) const; + private: + std::unordered_map> entries_; + }; + + struct CollisionProxy + { + CollisionProxy() : e1(nullptr), e2(nullptr), distance(0) {} + KinematicElement* e1; + KinematicElement* e2; + Eigen::Vector3d contact1; + Eigen::Vector3d normal1; + Eigen::Vector3d contact2; + Eigen::Vector3d normal2; + double distance; + + std::string print() const + { + std::stringstream ss; + if(e1 && e2) + { + ss<<"Proxy: '"<Segment.getName()<<"' - '"<Segment.getName()<<"', c1: "<& names) const +{ + names.clear(); + for(auto& it : entries_) + { + names.push_back(it.first); + } +} +bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2) const +{ + auto it = entries_.find(name1); + if(it==entries_.end()) return true; + return it->second.find(name2)==it->second.end(); +} + /////////////////////////////////////////////////////////////// /////////////////////// Collision Scene /////////////////////// /////////////////////////////////////////////////////////////// From e952fd06c76c546d36efe5449c862b5d2e4b7e7b Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 20:43:19 +0100 Subject: [PATCH 04/23] Reimplemented collision checking by directly updating FCL --- .../ompl_imp_solver/OMPLBaseStateSpace.cpp | 2 +- exotica/include/exotica/Scene.h | 318 +++---- exotica/src/Scene.cpp | 826 +++++++----------- exotica_python/src/Exotica.cpp | 61 +- 4 files changed, 442 insertions(+), 765 deletions(-) diff --git a/exotations/solvers/ompl_imp_solver/src/ompl_imp_solver/OMPLBaseStateSpace.cpp b/exotations/solvers/ompl_imp_solver/src/ompl_imp_solver/OMPLBaseStateSpace.cpp index c0f7040892..ec6df4ca5f 100644 --- a/exotations/solvers/ompl_imp_solver/src/ompl_imp_solver/OMPLBaseStateSpace.cpp +++ b/exotations/solvers/ompl_imp_solver/src/ompl_imp_solver/OMPLBaseStateSpace.cpp @@ -70,7 +70,7 @@ namespace exotica state, q); #endif - if (!prob_->isValid(q) || !prob_->getScene()->getCollisionScene()->isStateValid(self_collision_, margin_)) + if (!prob_->isValid(q) || !prob_->getScene()->getCollisionScene()->isStateValid(self_collision_)) { dist = -1; return false; diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 1337318aa8..1109e878dc 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -62,8 +62,7 @@ namespace exotica { - typedef std::vector geos_ptr; - typedef std::vector > fcls_ptr; + class Scene; class AllowedCollisionMatrix { @@ -109,94 +108,71 @@ namespace exotica { public: - CollisionScene(const std::string & scene_name); + struct CollisionData + { + CollisionData(CollisionScene* scene) : Scene(scene), Done(false), Self(true) {} - /** - * \brief Destructor - */ - virtual ~CollisionScene(); + fcl::CollisionRequest Request; + fcl::CollisionResult Result; + CollisionScene* Scene; + bool Done; + bool Self; + }; - /** - * \brief Initialisation function - * @param psmsg Moveit planning scene message - * @param joints Joint names - * @param mode Update mode - * @return Indication of success - */ - void initialise(const moveit_msgs::PlanningSceneConstPtr & psmsg, - const std::vector & joints, const std::string & mode, - BASE_TYPE base_type, robot_model::RobotModelPtr model_); + struct DistanceData + { + DistanceData(CollisionScene* scene) : Scene(scene), Self(true), Distance{1e300} {} - /** - * \brief Update the robot collision properties - * @param x Configuration - * @return Indication of success - */ - void update(Eigen::VectorXdRefConst x); + fcl::DistanceRequest Request; + fcl::DistanceResult Result; + CollisionScene* Scene; + std::vector Proxies; + double Distance; + bool Self; + }; - /** - * @brief Update an individual joint's value in the planning scene - * - * @param[in] joint The joint - * @param[in] value The value - */ - void update(std::string joint, double value); + struct ContactData + { + ContactData(CollisionScene* scene) : Scene(scene), Self(true), Distance{1e300} {} - /** - * \brief Get closest distance between two objects. - * @param o1 Name of object 1 - * @param o2 Name of object 2 - * @param d Signed closest distance (negative values indicate - * penetration) - */ - void getDistance(const std::string& o1, const std::string& o2, double& d, - double safeDist = 0.01); + fcl::DistanceRequest DistanceRequest; + fcl::DistanceResult DistanceResult; + fcl::CollisionRequest Request; + fcl::CollisionResult Result; + CollisionScene* Scene; + std::vector Proxies; + double Distance; + bool Self; + }; - /** - * @brief Get closest distance between two objects. - * @param o1 Name of object 1 - * @param o2 Name of object 2 - * @param d Signed closest distance (negative values indicate - * penetration) - * @param p1 If not in contact, closest point on o1. If in - * contact, position of the contact in world frame. - * @param p2 If not in contact, closest point on o2. If in - * contact, normalized contact normal. - * @param[in] safeDist Minimum distance to be considered safe, must be - * above 0. - */ - void getDistance(const std::string& o1, const std::string& o2, double& d, - Eigen::Vector3d& p1, Eigen::Vector3d& p2, - double safeDist = 0.01); + CollisionScene(const std::string & scene_name, robot_model::RobotModelPtr model, const std::string& root_name); /** - * \brief Check if the whole robot is valid (collision only) - * @param self Indicate if self collision check is required - * @return True, if the state is collision free. + * \brief Destructor */ - bool isStateValid(bool self = true, double dist = 0); + virtual ~CollisionScene(); - /** - * \brief Get closest distance between robot link and any other objects. - * @param link Robot link - * @param self Indicate if self collision check is required - * @param d Closest distance. Returns -1 if in collision. - * @param p1 Closest distance point on the link - * @param p2 Closest distance point on the other object - * @param norm Normal vector on robot link (p2-p1) - * @param c1 Center of the AABB of the colliding link - * @param c2 Center of the AABB of the other object - */ - void getRobotDistance(const std::string& link, bool self, double& d, - Eigen::Vector3d& p1, Eigen::Vector3d& p2, - Eigen::Vector3d& norm, Eigen::Vector3d& c1, - Eigen::Vector3d& c2, double safeDist); + visualization_msgs::Marker proxyToMarker(const std::vector& proxies); + + static bool isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionScene* scene); + static bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data); + static bool collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); + static bool collisionCallbackContacts(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); /** - * \brief Get current robot state - * @return Current robot state + * \brief Check if the whole robot is valid (collision only). + * @param self Indicate if self collision check is required. + * @return True, if the state is collision free.. */ - const robot_state::RobotState& getCurrentState(); + bool isStateValid(bool self = true); + + /// + /// \brief Computes collision distances. + /// \param self Indicate if self collision check is required. + /// \param computePenetrationDepth If set to true, accurate penetration depth is computed. + /// \return Collision proximity objectects for all colliding pairs of objects. + /// + std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true); /** * \brief Get the moveit planning scene @@ -204,27 +180,23 @@ namespace exotica */ planning_scene::PlanningScenePtr getPlanningScene(); - // Potentially deprecated - inline std::map& getFCLWorld() - { - return fcl_world_; - } - - // Potentially deprecated - inline std::map& getFCLRobot() - { - return fcl_robot_; - } - /** * @brief Gets the collision world links. * * @return The collision world links. */ - std::vector getCollisionWorldLinks() { - std::vector tmp; - for (auto& it : fcl_world_) tmp.push_back(it.first); - return tmp; + std::vector getCollisionWorldLinks() + { + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(!element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; } /** @@ -232,133 +204,56 @@ namespace exotica * * @return The collision robot links. */ - std::vector getCollisionRobotLinks() { - std::vector tmp; - for (auto& it : fcl_robot_) tmp.push_back(it.first); - return tmp; + std::vector getCollisionRobotLinks() + { + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; } - /** - * @brief Update the internal MoveIt planning scene from a - * moveit_msgs::PlanningSceneWorld - * - * @param[in] world moveit_msgs::PlanningSceneWorld - */ - void updateWorld(const moveit_msgs::PlanningSceneWorldConstPtr& world); - void getCollisionLinkTranslation(const std::string & name, - Eigen::Vector3d & translation); - void getWorldObjectTranslation(const std::string & name, - Eigen::Vector3d & translation); - void getTranslation(const std::string & name, - Eigen::Vector3d & translation); - int stateCheckCnt_; + Eigen::Vector3d getTranslation(const std::string & name); - /** - * @brief Reinitializes the FCL CollisionScene world links. Call this - * function if you update the MoveIt planning scene. - */ - void reinitializeCollisionWorld(); + inline void setACM(const AllowedCollisionMatrix& acm) + { + acm_ = acm; + } + + /// + /// \brief Creates the collision scene from kinematic elements. + /// \param objects Vector kinematic element pointers of collision objects. + /// + void updateCollisionObjects(const std::map>& objects); + + /// + /// \brief Updates collision object transformations from the kinematic tree. + /// + void updateCollisionObjectTransforms(); private: - /** - * @brief Reinitializes the FCL CollisionScene robot links. This - * function is automatically called when the scene is initialized via - * the initialise() method. - */ - void reinitializeCollisionRobot(); - - /** - * @brief Updates the transforms of the robot links in the FCL - * CollisionScene. This function is automatically called by the update() - * methods. - */ - void updateCollisionRobot(); - - /** - * \brief Get closest distance between two fcl objects - this is the main - * distance computation function called by the other methods. It will - * return -1 if the objects are in collision. - * @param fcl1 FCL object 1 - * @param fcl2 FCL object 2 - * @param req FCL distance request - * @param res FCL distance result - * @return Distance to collision (-1 if in collision) - */ - double distance(const fcls_ptr& fcl1, const fcls_ptr& fcl2, - const fcl::DistanceRequest& req, fcl::DistanceResult& res, - double safeDist); - - /** - * @brief Calculates the contact information. If there are multiple - * contacts between the two FCL objects, the information of the contact - * with the most penetration is returned. - * - * @param[in] fcl1 FCL object 1 - * @param[in] fcl2 FCL object 2 - * @param[in] req FCL collision request - * @param[in] res FCL distance result - * @param penetration_depth Maximum penetration depth - * @param pos Position of the contact in world frame - * @param norm Normalized contact normal - */ - void computeContact(const fcls_ptr& fcl1, const fcls_ptr& fcl2, - const fcl::CollisionRequest& req, - fcl::CollisionResult& res, double& penetration_depth, - Eigen::Vector3d& pos, Eigen::Vector3d& norm); - - /** - * @brief Gets the signed distance between two named objects. This - * function is the internal/private wrapper function providing - * functionality which is subsequently exposed via different overloads. - * - * @param[in] o1 Name of FCL link 1 - * @param[in] o2 Name of FCL link 2 - * @param d Signed distance - negative - * values indicate penetration - * @param[in] calculateContactInformation Whether to calculate the - * contact information, i.e. contact point and normals. - * @param[in] safeDist Safety distance, needs to be - * greater than 0. - * @param p1 Nearest point on link 1 or if - * in contact, the position of the contact in world frame. - * @param p2 Nearest point on link 2 or if - * in contact, the normalized contact normal. - */ - void getDistance(const std::string& o1, const std::string& o2, double& d, - const bool calculateContactInformation, - const double safeDist, Eigen::Vector3d& p1, - Eigen::Vector3d& p2); - - /// FCL collision object for the robot - std::map fcl_robot_; - - /// FCL collision object for the world - std::map fcl_world_; - - /// FCL collision geometry for the robot - std::map geo_robot_; - - /// FCL collision geometry for the world - std::map geo_world_; - - /// To correct FCL transform - std::map> trans_world_; + + static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + + std::map> fcl_cache_; + + std::vector fcl_objects_; /// Internal moveit planning scene planning_scene::PlanningScenePtr ps_; - /// Joint index in robot state - std::vector joint_index_; - /// Indicate if distance computation is required bool compute_dist; - /// The allowed collisiom matrix - collision_detection::AllowedCollisionMatrixPtr acm_; + std::string root_name_; - std::string scene_name_; - std::string world_joint_ = ""; - BASE_TYPE BaseType; + /// The allowed collisiom matrix + AllowedCollisionMatrix acm_; }; typedef std::shared_ptr CollisionScene_ptr; @@ -424,6 +319,15 @@ namespace exotica void detachObject(const std::string& name); bool hasAttachedObject(const std::string& name); + + /** + * @brief Update the internal MoveIt planning scene from a + * moveit_msgs::PlanningSceneWorld + * + * @param[in] world moveit_msgs::PlanningSceneWorld + */ + void updateWorld(const moveit_msgs::PlanningSceneWorldConstPtr& world); + BASE_TYPE getBaseType() { return BaseType; diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 9a3af1a7c2..4dd926d342 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -34,6 +34,9 @@ #include #include #include +#include +#include +#include namespace fcl_convert { @@ -57,6 +60,11 @@ namespace fcl_convert eigen(1) = fcl.data.vs[1]; eigen(2) = fcl.data.vs[2]; } + + fcl::Transform3f KDL2fcl(const KDL::Frame& frame) + { + return fcl::Transform3f(fcl::Matrix3f(frame.M.data[0], frame.M.data[1], frame.M.data[2], frame.M.data[3], frame.M.data[4], frame.M.data[5], frame.M.data[6], frame.M.data[7], frame.M.data[8]), fcl::Vec3f(frame.p.x(), frame.p.y(), frame.p.z())); + } } namespace exotica { @@ -104,520 +112,322 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const /////////////////////////////////////////////////////////////// /////////////////////// Collision Scene /////////////////////// /////////////////////////////////////////////////////////////// - CollisionScene::CollisionScene(const std::string & scene_name) - : compute_dist(true), stateCheckCnt_(0), scene_name_(scene_name) + CollisionScene::CollisionScene(const std::string & scene_name, robot_model::RobotModelPtr model, const std::string& root_name) + : compute_dist(true), ps_(new planning_scene::PlanningScene(model)), root_name_(root_name) { + HIGHLIGHT("FCL version: "< & joints, const std::string & mode, - BASE_TYPE base_type, robot_model::RobotModelPtr model_) + visualization_msgs::Marker CollisionScene::proxyToMarker(const std::vector& proxies) + { + visualization_msgs::Marker ret; + ret.header.frame_id = "exotica/"+root_name_; + ret.action = visualization_msgs::Marker::ADD; + ret.frame_locked = false; + ret.ns = "Proxies"; + ret.color.a=1.0; + ret.id=0; + ret.type = visualization_msgs::Marker::LINE_LIST; + ret.points.resize(proxies.size()*6); + ret.colors.resize(proxies.size()*6); + ret.scale.x = 0.005; + double normalLength = 0.05; + std_msgs::ColorRGBA normal = getColor(0.8,0.8,0.8); + std_msgs::ColorRGBA far = getColor(0.5,0.5,0.5); + std_msgs::ColorRGBA colliding = getColor(1,0,0); + for(int i=0; iFrame*KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); + KDL::Vector c2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); + KDL::Vector n1 = proxies[i].e1->Frame*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); + KDL::Vector n2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); + tf::pointKDLToMsg(c1, ret.points[i*6]); + tf::pointKDLToMsg(c2, ret.points[i*6+1]); + tf::pointKDLToMsg(c1, ret.points[i*6+2]); + tf::pointKDLToMsg(c1+n1*normalLength, ret.points[i*6+3]); + tf::pointKDLToMsg(c2, ret.points[i*6+4]); + tf::pointKDLToMsg(c2+n2*normalLength, ret.points[i*6+5]); + ret.colors[i*6] = proxies[i].distance>0?far:colliding; + ret.colors[i*6+1] = proxies[i].distance>0?far:colliding; + ret.colors[i*6+2]=ret.colors[i*6+3]=ret.colors[i*6+4]=ret.colors[i*6+5]=normal; + } + return ret; + } + + void CollisionScene::updateCollisionObjects(const std::map>& objects) { - ps_.reset(new planning_scene::PlanningScene(model_)); + fcl_objects_.resize(objects.size()); + int i=0; + for(const auto& object : objects) + { + std::shared_ptr new_object; - if (!acm_) - { - acm_.reset( - new collision_detection::AllowedCollisionMatrix( - ps_->getAllowedCollisionMatrix())); - } - ps_->setPlanningSceneMsg(*msg.get()); + const auto& cache_entry = fcl_cache_.find(object.first); + if(cache_entry == fcl_cache_.end()) + { + new_object = constructFclCollisionObject(object.second); + fcl_cache_[object.first] = new_object; + } + else + { + new_object = cache_entry->second; + } + fcl_objects_[i++] = new_object.get(); + } + } - if (model_->getSRDF()->getVirtualJoints().size() > 0) - world_joint_ = model_->getSRDF()->getVirtualJoints()[0].name_; + void CollisionScene::updateCollisionObjectTransforms() + { + for(fcl::CollisionObject* collision_object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(collision_object->getUserData()); + collision_object->setTransform(fcl_convert::KDL2fcl(element->Frame)); + collision_object->computeAABB(); + } + } - BaseType = base_type; - joint_index_.resize(joints.size()); + // This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' + std::shared_ptr CollisionScene::constructFclCollisionObject(std::shared_ptr element) + { + // Maybe use cache here? - for (std::size_t i = 0; - i < ps_->getCurrentState().getVariableNames().size(); i++) + shapes::ShapeConstPtr shape = element->Shape; + boost::shared_ptr geometry; + if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry { - for (std::size_t j = 0; j < joints.size(); j++) + // handle cases individually + switch (shape->type) { - if (ps_->getCurrentState().getVariableNames()[i] == joints[j]) + case shapes::PLANE: { - joint_index_[j] = i; - break; + const shapes::Plane* p = static_cast(shape.get()); + geometry.reset(new fcl::Plane(p->a, p->b, p->c, p->d)); } + break; + default: + break; } } - - if (mode.compare("Sampling") == 0) + else { - compute_dist = false; - INFO("Computing distance in Collision scene is Disabled"); + switch (shape->type) + { + case shapes::SPHERE: + { + const shapes::Sphere* s = static_cast(shape.get()); + geometry.reset(new fcl::Sphere(s->radius)); + } + break; + case shapes::BOX: + { + const shapes::Box* s = static_cast(shape.get()); + const double* size = s->size; + geometry.reset(new fcl::Box(size[0], size[1], size[2])); + } + break; + case shapes::CYLINDER: + { + const shapes::Cylinder* s = static_cast(shape.get()); + geometry.reset(new fcl::Cylinder(s->radius, s->length)); + } + break; + case shapes::CONE: + { + const shapes::Cone* s = static_cast(shape.get()); + geometry.reset(new fcl::Cone(s->radius, s->length)); + } + break; + case shapes::MESH: + { + fcl::BVHModel* g = new fcl::BVHModel(); + const shapes::Mesh* mesh = static_cast(shape.get()); + if (mesh->vertex_count > 0 && mesh->triangle_count > 0) + { + std::vector tri_indices(mesh->triangle_count); + for (unsigned int i = 0; i < mesh->triangle_count; ++i) + tri_indices[i] = + fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]); + + std::vector points(mesh->vertex_count); + for (unsigned int i = 0; i < mesh->vertex_count; ++i) + points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); + + g->beginModel(); + g->addSubModel(points, tri_indices); + g->endModel(); + } + geometry.reset(g); + } + break; + case shapes::OCTREE: + { + const shapes::OcTree* g = static_cast(shape.get()); + geometry.reset(new fcl::OcTree(g->octree)); + } + break; + default: + throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); + } } - else - INFO("Computing distance in Collision scene is Enabled"); + geometry->computeLocalAABB(); + geometry->setUserData(reinterpret_cast(element.get())); + std::shared_ptr ret(new fcl::CollisionObject(geometry)); + ret->setUserData(reinterpret_cast(element.get())); - if (compute_dist) { - reinitializeCollisionRobot(); - reinitializeCollisionWorld(); - } + return ret; } - void CollisionScene::reinitializeCollisionRobot() { - // Reinitialize robot - fcl_robot_.clear(); - geo_robot_.clear(); - ps_->getCurrentStateNonConst().update(true); - const std::vector& links = - ps_->getCollisionRobot() - ->getRobotModel() - ->getLinkModelsWithCollisionGeometry(); - for (std::size_t i = 0; i < links.size(); ++i) { - geo_robot_[links[i]->getName()] = geos_ptr(0); - fcl_robot_[links[i]->getName()] = fcls_ptr(0); - for (std::size_t j = 0; j < links[i]->getShapes().size(); ++j) { - shapes::ShapeConstPtr tmp_shape; - if (links[i]->getShapes()[j]->type != shapes::MESH) - tmp_shape = shapes::ShapeConstPtr( - shapes::createMeshFromShape(links[i]->getShapes()[j].get())); - else - tmp_shape = links[i]->getShapes()[j]; - if (!tmp_shape || !tmp_shape.get()) - throw_pretty("Shape could not be extracted"); - - collision_detection::FCLGeometryConstPtr g = - collision_detection::createCollisionGeometry(tmp_shape, links[i], - j); - if (g) { - geo_robot_.at(links[i]->getName()).push_back(g); - fcl::CollisionObject* tmp = new fcl::CollisionObject( - g->collision_geometry_, - collision_detection::transform2fcl( - ps_->getCurrentState().getCollisionBodyTransform( - g->collision_geometry_data_->ptr.link, - g->collision_geometry_data_->shape_index))); - fcl_robot_.at(links[i]->getName()) - .push_back(std::shared_ptr(tmp)); - - } else - throw_pretty("Unable to construct collision geometry for link " - << links[i]->getName().c_str()); - } - } - } + bool CollisionScene::isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionScene* scene) + { + KinematicElement* e1 = reinterpret_cast(o1->getUserData()); + KinematicElement* e2 = reinterpret_cast(o2->getUserData()); - void CollisionScene::reinitializeCollisionWorld() { - // Reinitialize world - fcl_world_.clear(); - geo_world_.clear(); - collision_detection::WorldConstPtr tmp_world = - ps_->getCollisionWorld()->getWorld(); - std::vector obj_id_ = tmp_world->getObjectIds(); - if (obj_id_.size() > 0) { - for (std::size_t i = 0; i < obj_id_.size(); ++i) { - std::size_t index_size = - tmp_world->getObject(obj_id_[i])->shapes_.size(); - fcl_world_[obj_id_[i]] = fcls_ptr(0); - geo_world_[obj_id_[i]] = geos_ptr(0); - trans_world_[obj_id_[i]] = std::vector(0); - for (std::size_t j = 0; j < index_size; j++) { - shapes::ShapeConstPtr tmp_shape; - - if (tmp_world->getObject(obj_id_[i])->shapes_[j]->type == - shapes::OCTREE) { - tmp_world->getObject(obj_id_[i])->shapes_[j]->print(); - tmp_shape = boost::static_pointer_cast( - tmp_world->getObject(obj_id_[i])->shapes_[j]); - } else { - if (tmp_world->getObject(obj_id_[i])->shapes_[j]->type != - shapes::MESH) { - tmp_shape = shapes::ShapeConstPtr(shapes::createMeshFromShape( - tmp_world->getObject(obj_id_[i])->shapes_[j].get())); - } else { - tmp_shape = tmp_world->getObject(obj_id_[i])->shapes_[j]; - } - } + bool isRobot1 = e1->isRobotLink || e1->ClosestRobotLink; + bool isRobot2 = e2->isRobotLink || e2->ClosestRobotLink; + // Don't check collisions between world objects + if(!isRobot1 && !isRobot2) return false; + // Skip self collisions if requested + if(isRobot1 && isRobot2 && !self) return false; + // Skip collisions between shapes within the same objects + if(e1->Parent==e2->Parent) return false; + // Skip collisions between bodies attached to the same object + if(e1->ClosestRobotLink&&e2->ClosestRobotLink&&e1->ClosestRobotLink==e2->ClosestRobotLink) return false; - if (!tmp_shape || !tmp_shape.get()) - throw_pretty("Could not extract shape"); - - collision_detection::FCLGeometryConstPtr g = - collision_detection::createCollisionGeometry( - tmp_shape, tmp_world->getObject(obj_id_[i]).get()); - geo_world_.at(obj_id_[i]).push_back(g); - trans_world_.at(obj_id_[i]) - .push_back(fcl::Transform3f(collision_detection::transform2fcl( - tmp_world->getObject(obj_id_[i])->shape_poses_[j]))); - fcl_world_.at(obj_id_[i]) - .push_back(std::shared_ptr( - new fcl::CollisionObject( - g->collision_geometry_, - collision_detection::transform2fcl( - tmp_world->getObject(obj_id_[i])->shape_poses_[j])))); - } + if(isRobot1 && isRobot2) + { + const std::string& name1 = e1->ClosestRobotLink?e1->ClosestRobotLink->Segment.getName():e1->Parent->Segment.getName(); + const std::string& name2 = e2->ClosestRobotLink?e2->ClosestRobotLink->Segment.getName():e2->Parent->Segment.getName(); + return scene->acm_.getAllowedCollision(name1, name2); } - } + return true; } - void CollisionScene::updateCollisionRobot() { - for (auto& it : fcl_robot_) { - for (std::size_t i = 0; i < it.second.size(); ++i) { - collision_detection::CollisionGeometryData* cd = - static_cast( - it.second[i]->collisionGeometry()->getUserData()); - it.second[i]->setTransform(collision_detection::transform2fcl( - ps_->getCurrentState().getCollisionBodyTransform(cd->ptr.link, - cd->shape_index))); - it.second[i]->getTransform().transform( - it.second[i]->collisionGeometry()->aabb_center); - } - } - } + bool CollisionScene::collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data) + { + CollisionData* data_ = reinterpret_cast(data); - void CollisionScene::updateWorld( - const moveit_msgs::PlanningSceneWorldConstPtr& world) { - ps_->processPlanningSceneWorldMsg(*world); + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - if (compute_dist) - reinitializeCollisionWorld(); + data_->Request.num_max_contacts = 1000; + data_->Result.clear(); + fcl::collide(o1,o2,data_->Request, data_->Result); + data_->Done = data_->Result.isCollision(); + return data_->Done; } - void CollisionScene::update(std::string joint, double value) { - try { - ps_->getCurrentStateNonConst().setVariablePosition(joint, value); - } catch (const std::exception& ex) { - throw_pretty("Exception while trying to update individual joint '" - << joint << "': " << ex.what()); - } - ps_->getCurrentStateNonConst().update(true); + bool CollisionScene::collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist) + { + DistanceData* data_ = reinterpret_cast(data); - // Update FCL robot link - if (compute_dist) - updateCollisionRobot(); - } + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + data_->Request.enable_nearest_points = false; + data_->Result.clear(); + fcl::distance(o1,o2,data_->Request, data_->Result); + int num_contacts = data_->Distance = std::min(data_->Distance, data_->Result.min_distance); - void CollisionScene::update(Eigen::VectorXdRefConst x) - { - if (joint_index_.size() != x.rows()) - throw_pretty("Size does not match, need vector size of " - << joint_index_.size() << " but " << x.rows() - << " is provided"); + CollisionProxy p; + p.e1 = reinterpret_cast(o1->getUserData()); + p.e2 = reinterpret_cast(o2->getUserData()); - if (BaseType == BASE_TYPE::FIXED) - { - for (std::size_t i = 0; i < joint_index_.size(); i++) - ps_->getCurrentStateNonConst().setVariablePosition(joint_index_[i], - x(i)); - } - else if (BaseType == BASE_TYPE::FLOATING) - { - ps_->getCurrentStateNonConst().setVariablePosition( - world_joint_ + "/trans_x", x(0)); - ps_->getCurrentStateNonConst().setVariablePosition( - world_joint_ + "/trans_y", x(1)); - ps_->getCurrentStateNonConst().setVariablePosition( - world_joint_ + "/trans_z", x(2)); - KDL::Rotation rot = KDL::Rotation::EulerZYX(x(3), x(4), x(5)); - Eigen::VectorXd quat(4); - rot.GetQuaternion(quat(0), quat(1), quat(2), quat(3)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/rot_x", - quat(0)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/rot_y", - quat(1)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/rot_z", - quat(2)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/rot_w", - quat(3)); - for (std::size_t i = 6; i < joint_index_.size(); i++) - ps_->getCurrentStateNonConst().setVariablePosition(joint_index_[i], - x(i)); - } - else if (BaseType == BASE_TYPE::PLANAR) - { - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/x", - x(0)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/y", - x(1)); - ps_->getCurrentStateNonConst().setVariablePosition(world_joint_ + "/theta", - x(2)); - for (std::size_t i = 3; i < joint_index_.size(); i++) - ps_->getCurrentStateNonConst().setVariablePosition(joint_index_[i], - x(i)); - } - ps_->getCurrentStateNonConst().update(true); - - // Update FCL robot state for collision distance computation - if (compute_dist) - updateCollisionRobot(); - } - - // Public overload returning only the distance - void CollisionScene::getDistance(const std::string& o1, const std::string& o2, - double& d, double safeDist) { - Eigen::Vector3d p1, p2; - getDistance(o1, o2, d, false, safeDist, p1, p2); - } - - // Public overload computing the nearest points - void CollisionScene::getDistance(const std::string& o1, const std::string& o2, - double& d, Eigen::Vector3d& p1, - Eigen::Vector3d& p2, double safeDist) { - getDistance(o1, o2, d, true, safeDist, p1, p2); - } - - // Private method to private functionality, wrapped via public overloads - void CollisionScene::getDistance(const std::string& o1, const std::string& o2, - double& d, - const bool calculateContactInformation, - const double safeDist, Eigen::Vector3d& p1, - Eigen::Vector3d& p2) { - fcls_ptr fcl1, fcl2; - if (fcl_robot_.find(o1) != fcl_robot_.end()) - fcl1 = fcl_robot_.at(o1); - else if (fcl_world_.find(o1) != fcl_world_.end()) - fcl1 = fcl_world_.at(o1); - else - throw_pretty("Object 1 not found!"); - - if (fcl_world_.find(o2) != fcl_world_.end()) - fcl2 = fcl_world_.at(o2); - else if (fcl_robot_.find(o2) != fcl_robot_.end()) - fcl2 = fcl_robot_.at(o2); - else - throw_pretty("Object 2 not found!"); - - fcl::DistanceRequest req; - req.enable_nearest_points = calculateContactInformation; - fcl::DistanceResult res; - d = distance(fcl1, fcl2, req, res, safeDist); - - // distance() returns either a distance > 0 or -1 if in collision - if (d > 0) { - d = res.min_distance; - - if (calculateContactInformation) { - fcl_convert::fcl2Eigen(res.nearest_points[0], p1); - fcl_convert::fcl2Eigen(res.nearest_points[1], p2); - } - } else if (d == -1) { // If d == -1, we need to compute the contact to get - // a penetration depth - fcl::CollisionRequest c_req; - c_req.enable_contact = true; - c_req.num_max_contacts = 500; - fcl::CollisionResult c_res; - computeContact(fcl1, fcl2, c_req, c_res, d, p1, p2); - d *= -1; - // ROS_INFO_STREAM("Penetration depth: " - // << d << " at world position=" << pos.transpose() - // << " with contact normal=" << norm.transpose()); - } else { - throw_pretty("We should never end up here. Why?"); - } + p.distance = data_->Result.min_distance; + fcl_convert::fcl2Eigen(data_->Result.nearest_points[0], p.contact1); + fcl_convert::fcl2Eigen(data_->Result.nearest_points[1], p.contact2); + p.normal1 = p.contact1.normalized(); + p.normal2 = p.contact2.normalized(); + p.distance = (p.contact1-p.contact2).norm(); + data_->Distance = std::min(data_->Distance, p.distance); + data_->Proxies.push_back(p); + + return false; } - bool CollisionScene::isStateValid(bool self, double dist) + bool CollisionScene::collisionCallbackContacts(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist) { - stateCheckCnt_++; - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - if (self) - { - ps_->checkSelfCollision(req, res, ps_->getCurrentState(), *acm_); - if (res.collision) - { - return false; - } - } - req.contacts = false; - if (dist > 0) req.distance = true; - ps_->getCollisionWorld()->checkRobotCollision(req, res, - *ps_->getCollisionRobot(), ps_->getCurrentState()); - return dist == 0 ? !res.collision : res.distance > dist; - } - - void CollisionScene::getRobotDistance(const std::string & link, bool self, - double & d, Eigen::Vector3d & p1, Eigen::Vector3d & p2, - Eigen::Vector3d & norm, Eigen::Vector3d & c1, Eigen::Vector3d & c2, - double safeDist) - { - fcls_ptr fcl_link; - if (fcl_robot_.find(link) != fcl_robot_.end()) - fcl_link = fcl_robot_.at(link); - else - { - throw_pretty("Link not found!"); - } - d = INFINITY; - fcl::DistanceRequest req(true); - fcl::DistanceResult res; - res.min_distance = INFINITY; - { - fcl::AABB sumAABB; - for (int i = 0; i < fcl_link.size(); i++) - { - fcl_link[i]->computeAABB(); - sumAABB += fcl_link[i]->getAABB(); - } - fcl_convert::fcl2Eigen(sumAABB.center(), c1); - } - if (self) - { - for (auto & it : fcl_robot_) + ContactData* data_ = reinterpret_cast(data); + + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + + data_->Request.enable_contact = true; + data_->Request.num_max_contacts = 1000; + data_->Result.clear(); + int num_contacts = fcl::collide(o1, o2, data_->Request, data_->Result); + CollisionProxy p; + p.e1 = reinterpret_cast(o1->getUserData()); + p.e2 = reinterpret_cast(o2->getUserData()); + + if(num_contacts>0) { - collision_detection::AllowedCollision::Type type = - collision_detection::AllowedCollision::ALWAYS; - if (link.compare(it.first) != 0 && acm_->getEntry(link, it.first, type)) - { - if (type == collision_detection::AllowedCollision::NEVER) + p.distance = -data_->Result.getContact(0).penetration_depth; + for(int i=0; iResult.getContact(i); + if(p.distance>-contact.penetration_depth) { - d = res.min_distance; - fcl_convert::fcl2Eigen( - it.second[i]->getTransform().transform( - it.second[i]->collisionGeometry()->aabb_center), c2); + p.distance=-contact.penetration_depth; + if(reinterpret_cast(contact.o1->getUserData())==p.e1) + { + fcl_convert::fcl2Eigen(contact.pos, p.contact1); + fcl_convert::fcl2Eigen(contact.normal, p.normal1); + p.normal1.normalize(); + } + else if(reinterpret_cast(contact.o2->getUserData())==p.e2) + { + fcl_convert::fcl2Eigen(contact.pos, p.contact2); + fcl_convert::fcl2Eigen(contact.normal, p.normal2); + p.normal2.normalize(); + } } - } } - else - { - ROS_INFO_STREAM_THROTTLE(2, - "Ignoring between "<setTransform(trans_world_.at(it.first)[i]); - it.second[i]->computeAABB(); } - - for (std::size_t i = 0; i < it.second.size(); i++) + else { - if (distance(fcl_link, it.second, req, res, safeDist) < 0) - { - d = -1; - fcl_convert::fcl2Eigen(it.second[i]->getAABB().center(), c2); - return; - } - else if (res.min_distance < d) - { - d = res.min_distance; - fcl_convert::fcl2Eigen(it.second[i]->getAABB().center(), c2); - } + data_->DistanceRequest.enable_nearest_points = true; + data_->DistanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD; + fcl::distance(o1,o2,data_->DistanceRequest, data_->DistanceResult); + fcl_convert::fcl2Eigen(data_->DistanceResult.nearest_points[0], p.contact1); + fcl_convert::fcl2Eigen(data_->DistanceResult.nearest_points[1], p.contact2); + p.normal1 = p.contact1.normalized(); + p.normal2 = p.contact2.normalized(); + p.distance = (p.contact1-p.contact2).norm(); } - } + data_->Distance = std::min(data_->Distance, p.distance); + data_->Proxies.push_back(p); - fcl_convert::fcl2Eigen(res.nearest_points[0], p1); - fcl_convert::fcl2Eigen(res.nearest_points[1], p2); + return false; + } - norm = p2 - p1; + bool CollisionScene::isStateValid(bool self) + { + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManager()); + manager->registerObjects(fcl_objects_); + CollisionData data(this); + data.Self = self; + manager->collide(&data, &CollisionScene::collisionCallback); + return !data.Result.isCollision(); } - double CollisionScene::distance(const fcls_ptr & fcl1, const fcls_ptr & fcl2, - const fcl::DistanceRequest & req, fcl::DistanceResult & res, - double safeDist) + std::vector CollisionScene::getCollisionDistance(bool self, bool computePenetrationDepth) { - fcl::DistanceResult tmp; - for (int i = 0; i < fcl1.size(); i++) - { - for (int j = 0; j < fcl2.size(); j++) + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManager()); + manager->registerObjects(fcl_objects_); + if(computePenetrationDepth) { - if (fcl1[i] == nullptr) throw_pretty("Object 1 not found!"); - - if (fcl2[j] == nullptr) throw_pretty("Object 2 not found!"); - - if (fcl2[j]->getAABB().distance(fcl2[j]->getAABB()) < safeDist) - { - if (fcl::distance(fcl1[i].get(), fcl2[j].get(), req, tmp) < 0) - { - throw_pretty( - "If this condition is triggered something has changed about " - "FCL's distance computation as this was not working in 0.3.4 " - "(Trusty). Need to reconsider logic - please open an issue on " - "GitHub."); - - res = tmp; - res.min_distance = -1; - return -1; - } - else - { - // If the current closest distance is less than previous, update the - // DistanceResult object - if (tmp.min_distance < res.min_distance) res = tmp; - - // The distance request returns 0 i.e. the two FCL objects are in - // contact. Now need to do a CollisionRequest in order to obtain the - // penetration depth and contact normals. However, we will return - // -1 here and have another method penetrationDepth carry out - // these computations. - if (res.min_distance == 0.0) return -1; - } - - tmp.clear(); - } + ContactData data(this); + data.Self = self; + manager->distance(&data, &CollisionScene::collisionCallbackContacts); + return data.Proxies; } - } - - return res.min_distance; - } - - void CollisionScene::computeContact( - const fcls_ptr& fcl1, const fcls_ptr& fcl2, - const fcl::CollisionRequest& req, fcl::CollisionResult& res, - double& penetration_depth, Eigen::Vector3d& pos, Eigen::Vector3d& norm) { - fcl::CollisionResult tmp; - penetration_depth = 0; - for (int i = 0; i < fcl1.size(); i++) { - for (int j = 0; j < fcl2.size(); j++) { - if (fcl1[i] == nullptr) throw_pretty("Object 1 not found!"); - if (fcl2[j] == nullptr) throw_pretty("Object 2 not found!"); - - std::size_t num_contacts = - fcl::collide(fcl1[i].get(), fcl2[j].get(), req, tmp); - - if (num_contacts == 0) { - throw_pretty("Objects are not in contact."); - } else { - ROS_INFO_STREAM("Objects have " << num_contacts - << " contact points."); - - // Iterate over contacts and compare maximum penetration - for (std::size_t k = 0; k < num_contacts; k++) { - auto& contact = tmp.getContact(k); - // ROS_INFO_STREAM("Contact #" << k << " has depth of " << contact.penetration_depth); - if (contact.penetration_depth > penetration_depth) { - penetration_depth = contact.penetration_depth; - fcl_convert::fcl2Eigen(contact.normal, norm); - fcl_convert::fcl2Eigen(contact.pos, pos); - norm = norm.normalized(); - res = tmp; - // ROS_INFO_STREAM("New deepest penetration depth: " << penetration_depth); - } - } - } - tmp.clear(); + else + { + DistanceData data(this); + data.Self = self; + manager->distance(&data, &CollisionScene::collisionCallbackDistance); + return data.Proxies; } - } - } - - const robot_state::RobotState& CollisionScene::getCurrentState() - { - return ps_->getCurrentState(); } planning_scene::PlanningScenePtr CollisionScene::getPlanningScene() @@ -625,39 +435,17 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const return ps_; } - void CollisionScene::getCollisionLinkTranslation(const std::string & name, - Eigen::Vector3d & translation) - { - if (fcl_robot_.find(name) == fcl_robot_.end()) throw_pretty("Robot not found!");; - std::map::iterator it = fcl_robot_.find(name); - fcl::AABB sumAABB; - for (int i = 0; i < it->second.size(); i++) - { - it->second[i]->computeAABB(); - sumAABB += it->second[i]->getAABB(); - } - fcl_convert::fcl2Eigen(sumAABB.center(), translation); - } - - void CollisionScene::getWorldObjectTranslation(const std::string & name, - Eigen::Vector3d & translation) - { - if (fcl_world_.find(name) == fcl_world_.end()) throw_pretty("Robot not found!");; - std::map::iterator it = fcl_world_.find(name); - fcl::AABB sumAABB; - for (int i = 0; i < it->second.size(); i++) - { - it->second[i]->computeAABB(); - sumAABB += it->second[i]->getAABB(); - } - fcl_convert::fcl2Eigen(sumAABB.center(), translation); - } - - void CollisionScene::getTranslation(const std::string & name, - Eigen::Vector3d & translation) + Eigen::Vector3d CollisionScene::getTranslation(const std::string & name) { - getCollisionLinkTranslation(name, translation); - getWorldObjectTranslation(name, translation); + for(fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->Segment.getName()==name) + { + return Eigen::Map(element->Frame.p.data); + } + } + throw_pretty("Robot not found!");; } /////////////////////////////////////////////////////////////// @@ -706,7 +494,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const kinematica_.Instantiate(init.JointGroup, model_, name_); group = model_->getJointModelGroup(init.JointGroup); - collision_scene_.reset(new CollisionScene(name_)); + collision_scene_.reset(new CollisionScene(name_, model_, kinematica_.getRootFrameName())); BaseType = kinematica_.getControlledBaseType(); @@ -721,13 +509,25 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const << "/PlanningScene'"); } + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); + + AllowedCollisionMatrix acm; + std::vector acm_names; + collision_scene_->getPlanningScene()->getAllowedCollisionMatrix().getAllEntryNames(acm_names); + for(auto& name1 : acm_names) { - planning_scene::PlanningScenePtr tmp(new planning_scene::PlanningScene(model_)); - moveit_msgs::PlanningScenePtr msg(new moveit_msgs::PlanningScene()); - tmp->getPlanningSceneMsg(*msg.get()); - collision_scene_->initialise(msg, kinematica_.getJointNames(), "", BaseType, model_); + for(auto& name2 : acm_names) + { + collision_detection::AllowedCollision::Type type = collision_detection::AllowedCollision::Type::ALWAYS; + collision_scene_->getPlanningScene()->getAllowedCollisionMatrix().getAllowedCollision(name1, name2, type); + if(type == collision_detection::AllowedCollision::Type::ALWAYS) + { + acm.setEntry(name1, name2); + } + } } - + collision_scene_->setACM(acm); + if (debug_) INFO_NAMED(name_, "Exotica Scene initialized"); } @@ -738,8 +538,8 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const void Scene::Update(Eigen::VectorXdRefConst x) { - collision_scene_->update(x); kinematica_.Update(x); + collision_scene_->updateCollisionObjectTransforms(); if (debug_) publishScene(); } @@ -758,14 +558,20 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const { moveit_msgs::PlanningScenePtr msg(new moveit_msgs::PlanningScene()); scene->getPlanningSceneMsg(*msg.get()); - collision_scene_->initialise(msg, kinematica_.getJointNames(), "", BaseType, model_); updateSceneFrames(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } void Scene::setCollisionScene(const moveit_msgs::PlanningSceneConstPtr & scene) { - collision_scene_->initialise(scene, kinematica_.getJointNames(), "", BaseType,model_); updateSceneFrames(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); + } + + void Scene::updateWorld(const moveit_msgs::PlanningSceneWorldConstPtr& world) + { + updateSceneFrames(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } CollisionScene_ptr & Scene::getCollisionScene() @@ -828,13 +634,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const // Update Kinematica internal state kinematica_.setModelState(x); - // Update Planning Scene State - int i = 0; - for (auto& joint : getModelJointNames()) - { - collision_scene_->update(joint, x(i)); - i++; - } + collision_scene_->updateCollisionObjectTransforms(); if (debug_) publishScene(); } @@ -843,9 +643,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const // Update Kinematica internal state kinematica_.setModelState(x); - // Update Planning Scene State - for (auto& joint : x) - collision_scene_->update(joint.first, joint.second); + collision_scene_->updateCollisionObjectTransforms(); if (debug_) publishScene(); } @@ -865,7 +663,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::stringstream ss(scene); getPlanningScene()->loadGeometryFromStream(ss); updateSceneFrames(); - collision_scene_->reinitializeCollisionWorld(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } void Scene::loadSceneFile(const std::string& file_name) @@ -873,7 +671,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::ifstream ss(parsePath(file_name)); getPlanningScene()->loadGeometryFromStream(ss); updateSceneFrames(); - collision_scene_->reinitializeCollisionWorld(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } std::string Scene::getScene() @@ -887,7 +685,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const { getPlanningScene()->removeAllCollisionObjects(); updateSceneFrames(); - collision_scene_->reinitializeCollisionWorld(); + collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } void Scene::updateSceneFrames() diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp index d96995c430..d4a62e8f3d 100644 --- a/exotica_python/src/Exotica.cpp +++ b/exotica_python/src/Exotica.cpp @@ -552,6 +552,19 @@ PYBIND11_MODULE(_pyexotica, module) samplingProblem.def("getSpaceDim", &SamplingProblem::getSpaceDim); samplingProblem.def("getBounds", &SamplingProblem::getBounds); + py::class_> proxy(module, "Proxy"); + proxy.def(py::init()); + proxy.def_readonly("Contact1", &CollisionProxy::contact1); + proxy.def_readonly("Contact2", &CollisionProxy::contact2); + proxy.def_readonly("Normal1", &CollisionProxy::normal1); + proxy.def_readonly("Normal2", &CollisionProxy::normal2); + proxy.def_readonly("Distance", &CollisionProxy::distance); + proxy.def_property_readonly("Object1", [](CollisionProxy* instance){ return (instance->e1 && instance->e2)?instance->e1->Segment.getName():std::string("");}); + proxy.def_property_readonly("Object2", [](CollisionProxy* instance){ return (instance->e1 && instance->e2)?instance->e2->Segment.getName():std::string("");}); + proxy.def_property_readonly("Transform1", [](CollisionProxy* instance){ return (instance->e1 && instance->e2)?instance->e1->Frame:KDL::Frame();}); + proxy.def_property_readonly("Transform2", [](CollisionProxy* instance){ return (instance->e1 && instance->e2)?instance->e2->Frame:KDL::Frame();}); + proxy.def("__repr__", &CollisionProxy::print); + py::class_, Object> scene(module, "Scene"); scene.def("Update", &Scene::Update); scene.def("getBaseType", &Scene::getBaseType); @@ -572,49 +585,11 @@ PYBIND11_MODULE(_pyexotica, module) scene.def("loadSceneFile", &Scene::loadSceneFile); scene.def("getScene", &Scene::getScene); scene.def("cleanScene", &Scene::cleanScene); - - // CollisionScene-related functions exposed via Scene - NB: may change in future - scene.def("isStateValid", [](Scene* instance) { - return instance->getCollisionScene()->isStateValid(); - }); - scene.def("isStateValid", [](Scene* instance, bool self, double dist) { - return instance->getCollisionScene()->isStateValid(self, dist); - }); - scene.def("updateWorld", [](Scene* instance, moveit_msgs::PlanningSceneWorld& world) { - moveit_msgs::PlanningSceneWorldConstPtr myPtr(new moveit_msgs::PlanningSceneWorld(world)); - instance->getCollisionScene()->updateWorld(myPtr); - }); - scene.def("getCollisionRobotLinks", [](Scene* instance) { - return instance->getCollisionScene()->getCollisionRobotLinks(); - }); - scene.def("getCollisionWorldLinks", [](Scene* instance) { - return instance->getCollisionScene()->getCollisionWorldLinks(); - }); - scene.def("getRobotDistance", [](Scene* instance, std::string link, bool self, double safeDist) { - double d; - Eigen::Vector3d p1, p2, norm, c1, c2; - instance->getCollisionScene()->getRobotDistance(link, self, d, p1, p2, norm, c1, c2, safeDist); - return py::make_tuple(d, norm); - }); - scene.def("getDistance", [](Scene* instance, std::string o1, std::string o2, - double safeDist) { - double d; - instance->getCollisionScene()->getDistance(o1, o2, d, safeDist); - return d; - }); - scene.def("getDistance", [](Scene* instance, std::string o1, std::string o2, - double safeDist, - bool computeContactInformation) { - double d; - Eigen::Vector3d p1 = Eigen::Vector3d::Zero(), - p2 = Eigen::Vector3d::Zero(); - if (computeContactInformation) { - instance->getCollisionScene()->getDistance(o1, o2, d, p1, p2, safeDist); - } else { - instance->getCollisionScene()->getDistance(o1, o2, d, safeDist); - } - return py::make_tuple(d, p1, p2); - }); + scene.def("isStateValid", [](Scene* instance, bool self) {return instance->getCollisionScene()->isStateValid(self);}, py::arg("self")=true); + scene.def("getCollisionDistance", [](Scene* instance, bool self, bool distances) {return instance->getCollisionScene()->getCollisionDistance(self, distances);}, py::arg("self")=true, py::arg("computeAccurateDistances")=true); + scene.def("updateWorld", &Scene::updateWorld); + scene.def("getCollisionRobotLinks", [](Scene* instance) {return instance->getCollisionScene()->getCollisionRobotLinks();}); + scene.def("getCollisionWorldLinks", [](Scene* instance) {return instance->getCollisionScene()->getCollisionWorldLinks();}); scene.def("getRootFrameName", &Scene::getRootFrameName); scene.def("getRootJointName", &Scene::getRootJointName); scene.def("getModelRootLinkName", &Scene::getModelRootLinkName); From 4b34c2e77d4ba434b816c5abb2aef9eac4dfdabd Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 20:43:47 +0100 Subject: [PATCH 05/23] Added collision proxy publishing --- exotica/include/exotica/Scene.h | 2 ++ exotica/src/Scene.cpp | 12 ++++++++++-- exotica_python/src/Exotica.cpp | 1 + 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 1109e878dc..54b6f2da18 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -334,6 +334,7 @@ namespace exotica } void publishScene(); + void publishProxies(const std::vector& proxies); void loadScene(const std::string& scene); void loadSceneFile(const std::string& file_name); std::string getScene(); @@ -359,6 +360,7 @@ namespace exotica /// Visual debug ros::Publisher ps_pub_; + ros::Publisher proxy_pub_; std::map attached_objects_; }; diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 4dd926d342..da5d6be663 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -499,8 +499,8 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const BaseType = kinematica_.getControlledBaseType(); if (Server::isRos()) { - ps_pub_ = Server::advertise( - name_ +(name_==""?"":"/")+"PlanningScene", 100, true); + ps_pub_ = Server::advertise(name_ +(name_==""?"":"/")+"PlanningScene", 100, true); + proxy_pub_ = Server::advertise(name_ +(name_==""?"":"/")+"CollisionProxies", 100, true); if (debug_) HIGHLIGHT_NAMED( name_, @@ -553,6 +553,14 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const } } + void Scene::publishProxies(const std::vector& proxies) + { + if(Server::isRos()) + { + proxy_pub_.publish(collision_scene_->proxyToMarker(proxies)); + } + } + void Scene::setCollisionScene( const planning_scene::PlanningSceneConstPtr & scene) { diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp index d4a62e8f3d..62482e98ad 100644 --- a/exotica_python/src/Exotica.cpp +++ b/exotica_python/src/Exotica.cpp @@ -577,6 +577,7 @@ PYBIND11_MODULE(_pyexotica, module) scene.def("setModelState", (void (Scene::*)(Eigen::VectorXdRefConst)) &Scene::setModelState); scene.def("setModelStateMap", (void (Scene::*)(std::map)) &Scene::setModelState); scene.def("publishScene", &Scene::publishScene); + scene.def("publishProxies", &Scene::publishProxies); scene.def("setCollisionScene", [](Scene* instance, moveit_msgs::PlanningScene& ps) { moveit_msgs::PlanningSceneConstPtr myPtr(new moveit_msgs::PlanningScene(ps)); instance->setCollisionScene(myPtr); From 336781af06905123cc5b5883ee2e4ef2c68e62ae Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 21:11:47 +0100 Subject: [PATCH 06/23] Moved moveit planning scene --- exotica/include/exotica/Scene.h | 13 +++------- exotica/src/Scene.cpp | 44 +++++++++++---------------------- 2 files changed, 18 insertions(+), 39 deletions(-) diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 54b6f2da18..49d4602d17 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -174,12 +174,6 @@ namespace exotica /// std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true); - /** - * \brief Get the moveit planning scene - * @return Moveit planning scene - */ - planning_scene::PlanningScenePtr getPlanningScene(); - /** * @brief Gets the collision world links. * @@ -244,9 +238,6 @@ namespace exotica std::vector fcl_objects_; - /// Internal moveit planning scene - planning_scene::PlanningScenePtr ps_; - /// Indicate if distance computation is required bool compute_dist; @@ -278,7 +269,6 @@ namespace exotica std::shared_ptr RequestKinematics(KinematicsRequest& Request); std::string getName(); virtual void Update(Eigen::VectorXdRefConst x); - void setCollisionScene(const planning_scene::PlanningSceneConstPtr & scene); void setCollisionScene(const moveit_msgs::PlanningSceneConstPtr & scene); CollisionScene_ptr & getCollisionScene(); std::string getRootFrameName(); @@ -358,6 +348,9 @@ namespace exotica /// The collision scene CollisionScene_ptr collision_scene_; + /// Internal moveit planning scene + planning_scene::PlanningScenePtr ps_; + /// Visual debug ros::Publisher ps_pub_; ros::Publisher proxy_pub_; diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index da5d6be663..c7037de0cd 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -113,7 +113,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const /////////////////////// Collision Scene /////////////////////// /////////////////////////////////////////////////////////////// CollisionScene::CollisionScene(const std::string & scene_name, robot_model::RobotModelPtr model, const std::string& root_name) - : compute_dist(true), ps_(new planning_scene::PlanningScene(model)), root_name_(root_name) + : compute_dist(true), root_name_(root_name) { HIGHLIGHT("FCL version: "<getJointModelGroup(init.JointGroup); + ps_.reset(new planning_scene::PlanningScene(model_)); collision_scene_.reset(new CollisionScene(name_, model_, kinematica_.getRootFrameName())); @@ -513,13 +510,13 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const AllowedCollisionMatrix acm; std::vector acm_names; - collision_scene_->getPlanningScene()->getAllowedCollisionMatrix().getAllEntryNames(acm_names); + ps_->getAllowedCollisionMatrix().getAllEntryNames(acm_names); for(auto& name1 : acm_names) { for(auto& name2 : acm_names) { collision_detection::AllowedCollision::Type type = collision_detection::AllowedCollision::Type::ALWAYS; - collision_scene_->getPlanningScene()->getAllowedCollisionMatrix().getAllowedCollision(name1, name2, type); + ps_->getAllowedCollisionMatrix().getAllowedCollision(name1, name2, type); if(type == collision_detection::AllowedCollision::Type::ALWAYS) { acm.setEntry(name1, name2); @@ -548,7 +545,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const if(Server::isRos()) { moveit_msgs::PlanningScene msg; - collision_scene_->getPlanningScene()->getPlanningSceneMsg(msg); + ps_->getPlanningSceneMsg(msg); ps_pub_.publish(msg); } } @@ -561,15 +558,6 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const } } - void Scene::setCollisionScene( - const planning_scene::PlanningSceneConstPtr & scene) - { - moveit_msgs::PlanningScenePtr msg(new moveit_msgs::PlanningScene()); - scene->getPlanningSceneMsg(*msg.get()); - updateSceneFrames(); - collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); - } - void Scene::setCollisionScene(const moveit_msgs::PlanningSceneConstPtr & scene) { updateSceneFrames(); @@ -604,7 +592,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const planning_scene::PlanningScenePtr Scene::getPlanningScene() { - return collision_scene_->getPlanningScene(); + return ps_; } exotica::KinematicTree & Scene::getSolver() @@ -669,7 +657,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const void Scene::loadScene(const std::string& scene) { std::stringstream ss(scene); - getPlanningScene()->loadGeometryFromStream(ss); + ps_->loadGeometryFromStream(ss); updateSceneFrames(); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } @@ -677,7 +665,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const void Scene::loadSceneFile(const std::string& file_name) { std::ifstream ss(parsePath(file_name)); - getPlanningScene()->loadGeometryFromStream(ss); + ps_->loadGeometryFromStream(ss); updateSceneFrames(); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } @@ -685,13 +673,13 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string Scene::getScene() { std::stringstream ss; - getPlanningScene()->saveGeometryToStream(ss); + ps_->saveGeometryToStream(ss); return ss.str(); } void Scene::cleanScene() { - getPlanningScene()->removeAllCollisionObjects(); + ps_->removeAllCollisionObjects(); updateSceneFrames(); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); } @@ -700,10 +688,8 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const { kinematica_.resetModel(); - planning_scene::PlanningScenePtr ps = collision_scene_->getPlanningScene(); - // Add world objects - for(auto& object : *ps->getWorld()) + for(auto& object : *ps_->getWorld()) { if(object.second->shapes_.size()) { @@ -723,16 +709,16 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const } // Add robot collision objects - ps->getCurrentStateNonConst().update(true); + ps_->getCurrentStateNonConst().update(true); const std::vector& links = - ps->getCollisionRobot()->getRobotModel()->getLinkModelsWithCollisionGeometry(); + ps_->getCollisionRobot()->getRobotModel()->getLinkModelsWithCollisionGeometry(); for (int i = 0; i < links.size(); ++i) { - Eigen::Affine3d objTransform = ps->getCurrentState().getGlobalLinkTransform(links[i]); + Eigen::Affine3d objTransform = ps_->getCurrentState().getGlobalLinkTransform(links[i]); for (int j = 0; j < links[i]->getShapes().size(); ++j) { - Eigen::Affine3d trans = objTransform.inverse()*ps->getCurrentState().getCollisionBodyTransform(links[i], j); + Eigen::Affine3d trans = objTransform.inverse()*ps_->getCurrentState().getCollisionBodyTransform(links[i], j); kinematica_.AddElement(links[i]->getName()+"_collision_"+std::to_string(j), trans, links[i]->getName(), links[i]->getShapes()[j]); } } From 743d3c1f817ed758a8456c503d29400be4a9d7cf Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 21:41:44 +0100 Subject: [PATCH 07/23] Cleaned up collision scene --- exotica/include/exotica/Scene.h | 2 +- exotica/src/Scene.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 49d4602d17..8679f3c826 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -145,7 +145,7 @@ namespace exotica bool Self; }; - CollisionScene(const std::string & scene_name, robot_model::RobotModelPtr model, const std::string& root_name); + CollisionScene(const std::string& root_name); /** * \brief Destructor diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index c7037de0cd..11a5347cca 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -112,10 +112,9 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const /////////////////////////////////////////////////////////////// /////////////////////// Collision Scene /////////////////////// /////////////////////////////////////////////////////////////// - CollisionScene::CollisionScene(const std::string & scene_name, robot_model::RobotModelPtr model, const std::string& root_name) - : compute_dist(true), root_name_(root_name) + CollisionScene::CollisionScene(const std::string& root_name) + : root_name_(root_name) { - HIGHLIGHT("FCL version: "<getJointModelGroup(init.JointGroup); ps_.reset(new planning_scene::PlanningScene(model_)); - collision_scene_.reset(new CollisionScene(name_, model_, kinematica_.getRootFrameName())); - BaseType = kinematica_.getControlledBaseType(); if (Server::isRos()) { @@ -506,6 +503,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const << "/PlanningScene'"); } + collision_scene_.reset(new CollisionScene(kinematica_.getRootFrameName())); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); AllowedCollisionMatrix acm; From 692b4253e1c2c1d0aef50e4547440a4680f3cba3 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 21:42:17 +0100 Subject: [PATCH 08/23] Moved KinematicElement to its own header --- exotica/include/exotica/KinematicElement.h | 55 ++++++++++++++++++++++ exotica/include/exotica/KinematicTree.h | 22 +-------- exotica/src/KinematicTree.cpp | 30 ------------ 3 files changed, 57 insertions(+), 50 deletions(-) create mode 100644 exotica/include/exotica/KinematicElement.h diff --git a/exotica/include/exotica/KinematicElement.h b/exotica/include/exotica/KinematicElement.h new file mode 100644 index 0000000000..6652d7ede3 --- /dev/null +++ b/exotica/include/exotica/KinematicElement.h @@ -0,0 +1,55 @@ +#ifndef KINEMATICELEMENT_H +#define KINEMATICELEMENT_H + +#include +#include +#include +#include + +class KinematicElement +{ +public: + KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment) : + Parent(parent), Segment(segment), Id(id), IsControlled(false), + ControlId(-1), Shape(nullptr), isRobotLink(false), ClosestRobotLink(nullptr) + { + } + inline void updateClosestRobotLink() + { + std::shared_ptr element = Parent; + ClosestRobotLink = nullptr; + while(element && element->Id>0) + { + if(element->isRobotLink) + { + ClosestRobotLink = element; + break; + } + element = element->Parent; + } + setChildrenClosestRobotLink(ClosestRobotLink); + } + + int Id; + int ControlId; + bool IsControlled; + std::shared_ptr Parent; + std::vector> Children; + std::shared_ptr ClosestRobotLink; + KDL::Segment Segment; + KDL::Frame Frame; + std::vector JointLimits; + shapes::ShapeConstPtr Shape; + bool isRobotLink; +private: + void setChildrenClosestRobotLink(std::shared_ptr element) + { + ClosestRobotLink = element; + for(auto& child : Children) + { + child->setChildrenClosestRobotLink(element); + } + } +}; + +#endif // KINEMATICELEMENT_H diff --git a/exotica/include/exotica/KinematicTree.h b/exotica/include/exotica/KinematicTree.h index b3be93ee0f..38e2292d1d 100644 --- a/exotica/include/exotica/KinematicTree.h +++ b/exotica/include/exotica/KinematicTree.h @@ -47,6 +47,8 @@ #include #include +#include + #define ROOT -1 //!< The value of the parent for the root segment namespace exotica @@ -100,26 +102,6 @@ namespace exotica std::vector Frames; //!< The segments to which the end-effectors are attached }; - class KinematicElement - { - public: - KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment); - void updateClosestRobotLink(); - int Id; - int ControlId; - bool IsControlled; - std::shared_ptr Parent; - std::vector> Children; - std::shared_ptr ClosestRobotLink; - KDL::Segment Segment; - KDL::Frame Frame; - std::vector JointLimits; - shapes::ShapeConstPtr Shape; - bool isRobotLink; - private: - void setChildrenClosestRobotLink(std::shared_ptr element); - }; - struct KinematicFrame { std::shared_ptr FrameA; diff --git a/exotica/src/KinematicTree.cpp b/exotica/src/KinematicTree.cpp index a408066bc2..bacf7f2f57 100644 --- a/exotica/src/KinematicTree.cpp +++ b/exotica/src/KinematicTree.cpp @@ -46,36 +46,6 @@ namespace exotica { -KinematicElement::KinematicElement(int id, std::shared_ptr parent, KDL::Segment segment) : Parent(parent), Segment(segment), Id(id), IsControlled(false), ControlId(-1), Shape(nullptr), isRobotLink(false), ClosestRobotLink(nullptr) -{ - -} - -void KinematicElement::setChildrenClosestRobotLink(std::shared_ptr element) -{ - ClosestRobotLink = element; - for(auto& child : Children) - { - child->setChildrenClosestRobotLink(element); - } -} - -void KinematicElement::updateClosestRobotLink() -{ - std::shared_ptr element = Parent; - ClosestRobotLink = nullptr; - while(element && element->Id>0) - { - if(element->isRobotLink) - { - ClosestRobotLink = element; - break; - } - element = element->Parent; - } - setChildrenClosestRobotLink(ClosestRobotLink); -} - KinematicResponse::KinematicResponse() : Flags(KIN_FK) { From 086469e9ad26d8ed9139c5c84c884034a018b50d Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Sun, 24 Sep 2017 22:15:26 +0100 Subject: [PATCH 09/23] Moved proxyToMarker to scene --- exotica/include/exotica/Scene.h | 3 +- exotica/src/Scene.cpp | 74 ++++++++++++++++----------------- 2 files changed, 38 insertions(+), 39 deletions(-) diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 8679f3c826..64163623ee 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -152,8 +152,6 @@ namespace exotica */ virtual ~CollisionScene(); - visualization_msgs::Marker proxyToMarker(const std::vector& proxies); - static bool isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionScene* scene); static bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data); static bool collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); @@ -325,6 +323,7 @@ namespace exotica void publishScene(); void publishProxies(const std::vector& proxies); + visualization_msgs::Marker proxyToMarker(const std::vector& proxies, const std::string& frame); void loadScene(const std::string& scene); void loadSceneFile(const std::string& file_name); std::string getScene(); diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 11a5347cca..6eb42e28fb 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -121,42 +121,6 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const { } - visualization_msgs::Marker CollisionScene::proxyToMarker(const std::vector& proxies) - { - visualization_msgs::Marker ret; - ret.header.frame_id = "exotica/"+root_name_; - ret.action = visualization_msgs::Marker::ADD; - ret.frame_locked = false; - ret.ns = "Proxies"; - ret.color.a=1.0; - ret.id=0; - ret.type = visualization_msgs::Marker::LINE_LIST; - ret.points.resize(proxies.size()*6); - ret.colors.resize(proxies.size()*6); - ret.scale.x = 0.005; - double normalLength = 0.05; - std_msgs::ColorRGBA normal = getColor(0.8,0.8,0.8); - std_msgs::ColorRGBA far = getColor(0.5,0.5,0.5); - std_msgs::ColorRGBA colliding = getColor(1,0,0); - for(int i=0; iFrame*KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); - KDL::Vector c2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); - KDL::Vector n1 = proxies[i].e1->Frame*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); - KDL::Vector n2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); - tf::pointKDLToMsg(c1, ret.points[i*6]); - tf::pointKDLToMsg(c2, ret.points[i*6+1]); - tf::pointKDLToMsg(c1, ret.points[i*6+2]); - tf::pointKDLToMsg(c1+n1*normalLength, ret.points[i*6+3]); - tf::pointKDLToMsg(c2, ret.points[i*6+4]); - tf::pointKDLToMsg(c2+n2*normalLength, ret.points[i*6+5]); - ret.colors[i*6] = proxies[i].distance>0?far:colliding; - ret.colors[i*6+1] = proxies[i].distance>0?far:colliding; - ret.colors[i*6+2]=ret.colors[i*6+3]=ret.colors[i*6+4]=ret.colors[i*6+5]=normal; - } - return ret; - } - void CollisionScene::updateCollisionObjects(const std::map>& objects) { fcl_objects_.resize(objects.size()); @@ -552,10 +516,46 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const { if(Server::isRos()) { - proxy_pub_.publish(collision_scene_->proxyToMarker(proxies)); + proxy_pub_.publish(proxyToMarker(proxies, kinematica_.getRootFrameName())); } } + visualization_msgs::Marker Scene::proxyToMarker(const std::vector& proxies, const std::string& frame) + { + visualization_msgs::Marker ret; + ret.header.frame_id = "exotica/"+frame; + ret.action = visualization_msgs::Marker::ADD; + ret.frame_locked = false; + ret.ns = "Proxies"; + ret.color.a=1.0; + ret.id=0; + ret.type = visualization_msgs::Marker::LINE_LIST; + ret.points.resize(proxies.size()*6); + ret.colors.resize(proxies.size()*6); + ret.scale.x = 0.005; + double normalLength = 0.05; + std_msgs::ColorRGBA normal = getColor(0.8,0.8,0.8); + std_msgs::ColorRGBA far = getColor(0.5,0.5,0.5); + std_msgs::ColorRGBA colliding = getColor(1,0,0); + for(int i=0; iFrame*KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); + KDL::Vector c2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); + KDL::Vector n1 = proxies[i].e1->Frame*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); + KDL::Vector n2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); + tf::pointKDLToMsg(c1, ret.points[i*6]); + tf::pointKDLToMsg(c2, ret.points[i*6+1]); + tf::pointKDLToMsg(c1, ret.points[i*6+2]); + tf::pointKDLToMsg(c1+n1*normalLength, ret.points[i*6+3]); + tf::pointKDLToMsg(c2, ret.points[i*6+4]); + tf::pointKDLToMsg(c2+n2*normalLength, ret.points[i*6+5]); + ret.colors[i*6] = proxies[i].distance>0?far:colliding; + ret.colors[i*6+1] = proxies[i].distance>0?far:colliding; + ret.colors[i*6+2]=ret.colors[i*6+3]=ret.colors[i*6+4]=ret.colors[i*6+5]=normal; + } + return ret; + } + void Scene::setCollisionScene(const moveit_msgs::PlanningSceneConstPtr & scene) { updateSceneFrames(); From dbc0f92634a2876097374642bc15fc129609007d Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Mon, 25 Sep 2017 00:52:51 +0100 Subject: [PATCH 10/23] Minor fixes --- exotica/include/exotica/Factory.h | 1 - exotica/include/exotica/KinematicElement.h | 15 ++++++++++----- exotica/include/exotica/Setup.h | 6 +++--- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/exotica/include/exotica/Factory.h b/exotica/include/exotica/Factory.h index f5eb18c5cd..6cf6a60193 100644 --- a/exotica/include/exotica/Factory.h +++ b/exotica/include/exotica/Factory.h @@ -37,7 +37,6 @@ #include #include -#include "exotica/Server.h" #include "exotica/Tools.h" #include diff --git a/exotica/include/exotica/KinematicElement.h b/exotica/include/exotica/KinematicElement.h index 6652d7ede3..76d98a0c65 100644 --- a/exotica/include/exotica/KinematicElement.h +++ b/exotica/include/exotica/KinematicElement.h @@ -5,6 +5,7 @@ #include #include #include +#include class KinematicElement { @@ -27,7 +28,7 @@ class KinematicElement } element = element->Parent; } - setChildrenClosestRobotLink(ClosestRobotLink); + setChildrenClosestRobotLink(); } int Id; @@ -42,12 +43,16 @@ class KinematicElement shapes::ShapeConstPtr Shape; bool isRobotLink; private: - void setChildrenClosestRobotLink(std::shared_ptr element) + inline void setChildrenClosestRobotLink() { - ClosestRobotLink = element; - for(auto& child : Children) + std::stack> elements; + for(auto child : Children) elements.push(child); + while(!elements.empty()) { - child->setChildrenClosestRobotLink(element); + auto parent = elements.top(); + elements.pop(); + parent->ClosestRobotLink = ClosestRobotLink; + for(auto child : parent->Children) elements.push(child); } } }; diff --git a/exotica/include/exotica/Setup.h b/exotica/include/exotica/Setup.h index 8ef2b2a714..4b9126a29e 100644 --- a/exotica/include/exotica/Setup.h +++ b/exotica/include/exotica/Setup.h @@ -66,9 +66,9 @@ namespace exotica } static void printSupportedClasses(); - static std::shared_ptr createSolver(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->solvers_.createInstance(prepend?"exotica/":""+type));} - static std::shared_ptr createMap(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->maps_.createInstance(prepend?"exotica/":""+type));} - static std::shared_ptr createProblem(const std::string & type, bool prepend = true) {return Instance()->problems_.createInstance(prepend?"exotica/":""+type);} + static std::shared_ptr createSolver(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->solvers_.createInstance((prepend?"exotica/":"")+type));} + static std::shared_ptr createMap(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->maps_.createInstance((prepend?"exotica/":"")+type));} + static std::shared_ptr createProblem(const std::string & type, bool prepend = true) {return Instance()->problems_.createInstance((prepend?"exotica/":"")+type);} static std::vector getSolvers(); static std::vector getProblems(); static std::vector getMaps(); From c30e0eabf9f76f23a1207e348c9791bdf47fe6e6 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Mon, 25 Sep 2017 00:56:23 +0100 Subject: [PATCH 11/23] Moved collision checking into a plugin --- exotations/collision_scene_fcl/CMakeLists.txt | 38 ++ .../collision_scene_fcl/exotica_plugins.xml | 5 + .../collision_scene_fcl/CollisionSceneFCL.h | 146 +++++++ exotations/collision_scene_fcl/package.xml | 18 + .../src/CollisionSceneFCL.cpp | 260 ++++++++++++ exotica/include/exotica/CollisionScene.h | 137 +++++++ exotica/include/exotica/Scene.h | 205 +--------- exotica/include/exotica/Setup.h | 3 + exotica/src/Scene.cpp | 373 +----------------- exotica/src/Setup.cpp | 9 +- exotica_python/src/Exotica.cpp | 1 + 11 files changed, 619 insertions(+), 576 deletions(-) create mode 100644 exotations/collision_scene_fcl/CMakeLists.txt create mode 100644 exotations/collision_scene_fcl/exotica_plugins.xml create mode 100644 exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h create mode 100644 exotations/collision_scene_fcl/package.xml create mode 100644 exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp create mode 100644 exotica/include/exotica/CollisionScene.h diff --git a/exotations/collision_scene_fcl/CMakeLists.txt b/exotations/collision_scene_fcl/CMakeLists.txt new file mode 100644 index 0000000000..6ed7e7ec16 --- /dev/null +++ b/exotations/collision_scene_fcl/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.8.3) +project(collision_scene_fcl) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + exotica + geometric_shapes + fcl +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES collision_scene_fcl + CATKIN_DEPENDS exotica geometric_shapes fcl + +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/CollisionSceneFCL.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(DIRECTORY include/ DESTINATION include) +install(FILES exotica_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/exotations/collision_scene_fcl/exotica_plugins.xml b/exotations/collision_scene_fcl/exotica_plugins.xml new file mode 100644 index 0000000000..6646d837a5 --- /dev/null +++ b/exotations/collision_scene_fcl/exotica_plugins.xml @@ -0,0 +1,5 @@ + + + FCL collision scene + + diff --git a/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h b/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h new file mode 100644 index 0000000000..26dc4469db --- /dev/null +++ b/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h @@ -0,0 +1,146 @@ +/* + * Author: Vladimir Ivan + * + * Copyright (c) 2017, University Of Edinburgh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef COLLISIONSCENEFCL_H +#define COLLISIONSCENEFCL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace exotica +{ +class CollisionSceneFCL : public CollisionScene +{ +public: + + struct CollisionData + { + CollisionData(CollisionSceneFCL* scene) : Scene(scene), Done(false), Self(true) {} + + fcl::CollisionRequest Request; + fcl::CollisionResult Result; + CollisionSceneFCL* Scene; + bool Done; + bool Self; + }; + + CollisionSceneFCL(); + + /** + * \brief Destructor + */ + virtual ~CollisionSceneFCL(); + + static bool isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionSceneFCL* scene); + static bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data); + static bool collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); + static bool collisionCallbackContacts(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); + + /** + * \brief Check if the whole robot is valid (collision only). + * @param self Indicate if self collision check is required. + * @return True, if the state is collision free.. + */ + virtual bool isStateValid(bool self = true); + + /** + * @brief Gets the collision world links. + * + * @return The collision world links. + */ + virtual std::vector getCollisionWorldLinks() + { + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(!element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; + } + + /** + * @brief Gets the collision robot links. + * + * @return The collision robot links. + */ + virtual std::vector getCollisionRobotLinks() + { + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; + } + + virtual Eigen::Vector3d getTranslation(const std::string & name); + + /// + /// \brief Creates the collision scene from kinematic elements. + /// \param objects Vector kinematic element pointers of collision objects. + /// + virtual void updateCollisionObjects(const std::map>& objects); + + /// + /// \brief Updates collision object transformations from the kinematic tree. + /// + virtual void updateCollisionObjectTransforms(); + +private: + + static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + + std::map> fcl_cache_; + + std::vector fcl_objects_; +}; + +typedef std::shared_ptr CollisionSceneFCL_ptr; +} + +#endif // COLLISIONSCENEFCL_H diff --git a/exotations/collision_scene_fcl/package.xml b/exotations/collision_scene_fcl/package.xml new file mode 100644 index 0000000000..c10cc7608a --- /dev/null +++ b/exotations/collision_scene_fcl/package.xml @@ -0,0 +1,18 @@ + + + collision_scene_fcl + 0.0.0 + The collision checking using the FCL library. + Vladimir Ivan + + BSD + + catkin + exotica + geometric_shapes + fcl + + + + + diff --git a/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp b/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp new file mode 100644 index 0000000000..09fc3fc1e9 --- /dev/null +++ b/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp @@ -0,0 +1,260 @@ +/* + * Author: Vladimir Ivan + * + * Copyright (c) 2017, University Of Edinburgh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +REGISTER_COLLISION_SCENE_TYPE("CollisionSceneFCL", exotica::CollisionSceneFCL) + +namespace fcl_convert +{ +void fcl2Eigen(const fcl::Vec3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl.data.vs[0]; + eigen(1) = fcl.data.vs[1]; + eigen(2) = fcl.data.vs[2]; +} + +void fcl2Eigen(const fcl::Transform3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl.getTranslation().data.vs[0]; + eigen(1) = fcl.getTranslation().data.vs[1]; + eigen(2) = fcl.getTranslation().data.vs[2]; +} + +void fcl2EigenTranslation(const fcl::Vec3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl.data.vs[0]; + eigen(1) = fcl.data.vs[1]; + eigen(2) = fcl.data.vs[2]; +} + +fcl::Transform3f KDL2fcl(const KDL::Frame& frame) +{ + return fcl::Transform3f(fcl::Matrix3f(frame.M.data[0], frame.M.data[1], frame.M.data[2], frame.M.data[3], frame.M.data[4], frame.M.data[5], frame.M.data[6], frame.M.data[7], frame.M.data[8]), fcl::Vec3f(frame.p.x(), frame.p.y(), frame.p.z())); +} +} + +namespace exotica +{ +CollisionSceneFCL::CollisionSceneFCL() +{ +} + +CollisionSceneFCL::~CollisionSceneFCL() +{ +} + +void CollisionSceneFCL::updateCollisionObjects(const std::map>& objects) +{ + fcl_objects_.resize(objects.size()); + int i=0; + for(const auto& object : objects) + { + std::shared_ptr new_object; + + const auto& cache_entry = fcl_cache_.find(object.first); + if(cache_entry == fcl_cache_.end()) + { + new_object = constructFclCollisionObject(object.second); + fcl_cache_[object.first] = new_object; + } + else + { + new_object = cache_entry->second; + } + fcl_objects_[i++] = new_object.get(); + } +} + +void CollisionSceneFCL::updateCollisionObjectTransforms() +{ + for(fcl::CollisionObject* collision_object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(collision_object->getUserData()); + collision_object->setTransform(fcl_convert::KDL2fcl(element->Frame)); + collision_object->computeAABB(); + } +} + +// This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' +std::shared_ptr CollisionSceneFCL::constructFclCollisionObject(std::shared_ptr element) +{ + // Maybe use cache here? + + shapes::ShapeConstPtr shape = element->Shape; + boost::shared_ptr geometry; + if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry + { + // handle cases individually + switch (shape->type) + { + case shapes::PLANE: + { + const shapes::Plane* p = static_cast(shape.get()); + geometry.reset(new fcl::Plane(p->a, p->b, p->c, p->d)); + } + break; + default: + break; + } + } + else + { + switch (shape->type) + { + case shapes::SPHERE: + { + const shapes::Sphere* s = static_cast(shape.get()); + geometry.reset(new fcl::Sphere(s->radius)); + } + break; + case shapes::BOX: + { + const shapes::Box* s = static_cast(shape.get()); + const double* size = s->size; + geometry.reset(new fcl::Box(size[0], size[1], size[2])); + } + break; + case shapes::CYLINDER: + { + const shapes::Cylinder* s = static_cast(shape.get()); + geometry.reset(new fcl::Cylinder(s->radius, s->length)); + } + break; + case shapes::CONE: + { + const shapes::Cone* s = static_cast(shape.get()); + geometry.reset(new fcl::Cone(s->radius, s->length)); + } + break; + case shapes::MESH: + { + fcl::BVHModel* g = new fcl::BVHModel(); + const shapes::Mesh* mesh = static_cast(shape.get()); + if (mesh->vertex_count > 0 && mesh->triangle_count > 0) + { + std::vector tri_indices(mesh->triangle_count); + for (unsigned int i = 0; i < mesh->triangle_count; ++i) + tri_indices[i] = + fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]); + + std::vector points(mesh->vertex_count); + for (unsigned int i = 0; i < mesh->vertex_count; ++i) + points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); + + g->beginModel(); + g->addSubModel(points, tri_indices); + g->endModel(); + } + geometry.reset(g); + } + break; + case shapes::OCTREE: + { + const shapes::OcTree* g = static_cast(shape.get()); + geometry.reset(new fcl::OcTree(g->octree)); + } + break; + default: + throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); + } + } + geometry->computeLocalAABB(); + geometry->setUserData(reinterpret_cast(element.get())); + std::shared_ptr ret(new fcl::CollisionObject(geometry)); + ret->setUserData(reinterpret_cast(element.get())); + + return ret; +} + +bool CollisionSceneFCL::isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionSceneFCL* scene) +{ + KinematicElement* e1 = reinterpret_cast(o1->getUserData()); + KinematicElement* e2 = reinterpret_cast(o2->getUserData()); + + bool isRobot1 = e1->isRobotLink || e1->ClosestRobotLink; + bool isRobot2 = e2->isRobotLink || e2->ClosestRobotLink; + // Don't check collisions between world objects + if(!isRobot1 && !isRobot2) return false; + // Skip self collisions if requested + if(isRobot1 && isRobot2 && !self) return false; + // Skip collisions between shapes within the same objects + if(e1->Parent==e2->Parent) return false; + // Skip collisions between bodies attached to the same object + if(e1->ClosestRobotLink&&e2->ClosestRobotLink&&e1->ClosestRobotLink==e2->ClosestRobotLink) return false; + + if(isRobot1 && isRobot2) + { + const std::string& name1 = e1->ClosestRobotLink?e1->ClosestRobotLink->Segment.getName():e1->Parent->Segment.getName(); + const std::string& name2 = e2->ClosestRobotLink?e2->ClosestRobotLink->Segment.getName():e2->Parent->Segment.getName(); + return scene->acm_.getAllowedCollision(name1, name2); + } + return true; +} + +bool CollisionSceneFCL::collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data) +{ + CollisionData* data_ = reinterpret_cast(data); + + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + + data_->Request.num_max_contacts = 1000; + data_->Result.clear(); + fcl::collide(o1,o2,data_->Request, data_->Result); + data_->Done = data_->Result.isCollision(); + return data_->Done; +} + +bool CollisionSceneFCL::isStateValid(bool self) +{ + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManager()); + manager->registerObjects(fcl_objects_); + CollisionData data(this); + data.Self = self; + manager->collide(&data, &CollisionSceneFCL::collisionCallback); + return !data.Result.isCollision(); +} + +Eigen::Vector3d CollisionSceneFCL::getTranslation(const std::string & name) +{ + for(fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->Segment.getName()==name) + { + return Eigen::Map(element->Frame.p.data); + } + } + throw_pretty("Robot not found!");; +} +} diff --git a/exotica/include/exotica/CollisionScene.h b/exotica/include/exotica/CollisionScene.h new file mode 100644 index 0000000000..3e39133b91 --- /dev/null +++ b/exotica/include/exotica/CollisionScene.h @@ -0,0 +1,137 @@ +#ifndef COLLISIONSCENE_H +#define COLLISIONSCENE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define REGISTER_COLLISION_SCENE_TYPE(TYPE, DERIV) EXOTICA_REGISTER(exotica::CollisionScene, TYPE, DERIV) +namespace exotica +{ + +class AllowedCollisionMatrix +{ +public: + AllowedCollisionMatrix() {} + AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) {entries_ = acm.entries_;} + inline void clear() {entries_.clear();} + inline bool hasEntry(const std::string& name) const {return entries_.find(name)==entries_.end();} + inline void setEntry(const std::string& name1, const std::string& name2) {entries_[name1].insert(name2);} + inline void getAllEntryNames(std::vector& names) const + { + names.clear(); + for(auto& it : entries_) + { + names.push_back(it.first); + } + } + inline bool getAllowedCollision(const std::string& name1, const std::string& name2) const + { + auto it = entries_.find(name1); + if(it==entries_.end()) return true; + return it->second.find(name2)==it->second.end(); + } +private: + std::unordered_map> entries_; +}; + +struct CollisionProxy +{ + CollisionProxy() : e1(nullptr), e2(nullptr), distance(0) {} + KinematicElement* e1; + KinematicElement* e2; + Eigen::Vector3d contact1; + Eigen::Vector3d normal1; + Eigen::Vector3d contact2; + Eigen::Vector3d normal2; + double distance; + + inline std::string print() const + { + std::stringstream ss; + if(e1 && e2) + { + ss<<"Proxy: '"<Segment.getName()<<"' - '"<Segment.getName()<<"', c1: "< getCollisionDistance(bool self, bool computePenetrationDepth = true) {throw_pretty("Not implemented!");} + + /** + * @brief Gets the collision world links. + * @return The collision world links. + */ + virtual std::vector getCollisionWorldLinks() = 0; + + /** + * @brief Gets the collision robot links. + * @return The collision robot links. + */ + virtual std::vector getCollisionRobotLinks() = 0; + + virtual Eigen::Vector3d getTranslation(const std::string & name) = 0; + + inline void setACM(const AllowedCollisionMatrix& acm) + { + acm_ = acm; + } + + /// + /// \brief Creates the collision scene from kinematic elements. + /// \param objects Vector kinematic element pointers of collision objects. + /// + virtual void updateCollisionObjects(const std::map>& objects) = 0; + + /// + /// \brief Updates collision object transformations from the kinematic tree. + /// + virtual void updateCollisionObjectTransforms() = 0; + +protected: + + /// The allowed collisiom matrix + AllowedCollisionMatrix acm_; +}; + +typedef exotica::Factory CollisionScene_fac; +typedef std::shared_ptr CollisionScene_ptr; +} + +#endif // COLLISIONSCENE_H diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h index 64163623ee..806f2ecedd 100644 --- a/exotica/include/exotica/Scene.h +++ b/exotica/include/exotica/Scene.h @@ -38,215 +38,12 @@ #include "exotica/KinematicTree.h" #include #include - -// For collision -#include -#include -#include -#include -#include -#include -#include -#include +#include #include #include -#include -#include -#include - -#include -#include -#include -#include - namespace exotica { - class Scene; - - class AllowedCollisionMatrix - { - public: - AllowedCollisionMatrix(); - AllowedCollisionMatrix(const AllowedCollisionMatrix& acm); - void clear(); - bool hasEntry(const std::string& name) const; - void setEntry(const std::string& name1, const std::string& name2); - void getAllEntryNames(std::vector& names) const; - bool getAllowedCollision(const std::string& name1, const std::string& name2) const; - private: - std::unordered_map> entries_; - }; - - struct CollisionProxy - { - CollisionProxy() : e1(nullptr), e2(nullptr), distance(0) {} - KinematicElement* e1; - KinematicElement* e2; - Eigen::Vector3d contact1; - Eigen::Vector3d normal1; - Eigen::Vector3d contact2; - Eigen::Vector3d normal2; - double distance; - - std::string print() const - { - std::stringstream ss; - if(e1 && e2) - { - ss<<"Proxy: '"<Segment.getName()<<"' - '"<Segment.getName()<<"', c1: "< Proxies; - double Distance; - bool Self; - }; - - struct ContactData - { - ContactData(CollisionScene* scene) : Scene(scene), Self(true), Distance{1e300} {} - - fcl::DistanceRequest DistanceRequest; - fcl::DistanceResult DistanceResult; - fcl::CollisionRequest Request; - fcl::CollisionResult Result; - CollisionScene* Scene; - std::vector Proxies; - double Distance; - bool Self; - }; - - CollisionScene(const std::string& root_name); - - /** - * \brief Destructor - */ - virtual ~CollisionScene(); - - static bool isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionScene* scene); - static bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data); - static bool collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); - static bool collisionCallbackContacts(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist); - - /** - * \brief Check if the whole robot is valid (collision only). - * @param self Indicate if self collision check is required. - * @return True, if the state is collision free.. - */ - bool isStateValid(bool self = true); - - /// - /// \brief Computes collision distances. - /// \param self Indicate if self collision check is required. - /// \param computePenetrationDepth If set to true, accurate penetration depth is computed. - /// \return Collision proximity objectects for all colliding pairs of objects. - /// - std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true); - - /** - * @brief Gets the collision world links. - * - * @return The collision world links. - */ - std::vector getCollisionWorldLinks() - { - std::vector tmp; - for (fcl::CollisionObject* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(!element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } - - /** - * @brief Gets the collision robot links. - * - * @return The collision robot links. - */ - std::vector getCollisionRobotLinks() - { - std::vector tmp; - for (fcl::CollisionObject* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } - - Eigen::Vector3d getTranslation(const std::string & name); - - inline void setACM(const AllowedCollisionMatrix& acm) - { - acm_ = acm; - } - - /// - /// \brief Creates the collision scene from kinematic elements. - /// \param objects Vector kinematic element pointers of collision objects. - /// - void updateCollisionObjects(const std::map>& objects); - - /// - /// \brief Updates collision object transformations from the kinematic tree. - /// - void updateCollisionObjectTransforms(); - - private: - - static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); - - std::map> fcl_cache_; - - std::vector fcl_objects_; - - /// Indicate if distance computation is required - bool compute_dist; - - std::string root_name_; - - /// The allowed collisiom matrix - AllowedCollisionMatrix acm_; - }; - - typedef std::shared_ptr CollisionScene_ptr; - struct AttachedObject { AttachedObject() : Parent("") {} diff --git a/exotica/include/exotica/Setup.h b/exotica/include/exotica/Setup.h index 4b9126a29e..65c3c224a9 100644 --- a/exotica/include/exotica/Setup.h +++ b/exotica/include/exotica/Setup.h @@ -69,9 +69,11 @@ namespace exotica static std::shared_ptr createSolver(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->solvers_.createInstance((prepend?"exotica/":"")+type));} static std::shared_ptr createMap(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->maps_.createInstance((prepend?"exotica/":"")+type));} static std::shared_ptr createProblem(const std::string & type, bool prepend = true) {return Instance()->problems_.createInstance((prepend?"exotica/":"")+type);} + static std::shared_ptr createCollisionScene(const std::string & type, bool prepend = true) {return to_std_ptr(Instance()->scenes_.createInstance((prepend?"exotica/":"")+type));} static std::vector getSolvers(); static std::vector getProblems(); static std::vector getMaps(); + static std::vector getCollisionScenes(); static std::vector getInitializers(); static std::shared_ptr createSolver(const Initializer& init) @@ -108,6 +110,7 @@ namespace exotica pluginlib::ClassLoader solvers_; pluginlib::ClassLoader maps_; + pluginlib::ClassLoader scenes_; PlanningProblem_fac problems_; }; diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 6eb42e28fb..22b1d729f2 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -31,382 +31,13 @@ */ #include "exotica/Scene.h" +#include #include #include #include -#include -#include -#include -namespace fcl_convert -{ - void fcl2Eigen(const fcl::Vec3f & fcl, Eigen::Vector3d & eigen) - { - eigen(0) = fcl.data.vs[0]; - eigen(1) = fcl.data.vs[1]; - eigen(2) = fcl.data.vs[2]; - } - - void fcl2Eigen(const fcl::Transform3f & fcl, Eigen::Vector3d & eigen) - { - eigen(0) = fcl.getTranslation().data.vs[0]; - eigen(1) = fcl.getTranslation().data.vs[1]; - eigen(2) = fcl.getTranslation().data.vs[2]; - } - - void fcl2EigenTranslation(const fcl::Vec3f & fcl, Eigen::Vector3d & eigen) - { - eigen(0) = fcl.data.vs[0]; - eigen(1) = fcl.data.vs[1]; - eigen(2) = fcl.data.vs[2]; - } - - fcl::Transform3f KDL2fcl(const KDL::Frame& frame) - { - return fcl::Transform3f(fcl::Matrix3f(frame.M.data[0], frame.M.data[1], frame.M.data[2], frame.M.data[3], frame.M.data[4], frame.M.data[5], frame.M.data[6], frame.M.data[7], frame.M.data[8]), fcl::Vec3f(frame.p.x(), frame.p.y(), frame.p.z())); - } -} namespace exotica { - -AllowedCollisionMatrix::AllowedCollisionMatrix() -{ - -} - -AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) -{ - entries_ = acm.entries_; -} - -void AllowedCollisionMatrix::clear() -{ - entries_.clear(); -} - -bool AllowedCollisionMatrix::hasEntry(const std::string& name) const -{ - return entries_.find(name)==entries_.end(); -} - -void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2) -{ - entries_[name1].insert(name2); -} - -void AllowedCollisionMatrix::getAllEntryNames(std::vector& names) const -{ - names.clear(); - for(auto& it : entries_) - { - names.push_back(it.first); - } -} -bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2) const -{ - auto it = entries_.find(name1); - if(it==entries_.end()) return true; - return it->second.find(name2)==it->second.end(); -} - -/////////////////////////////////////////////////////////////// -/////////////////////// Collision Scene /////////////////////// -/////////////////////////////////////////////////////////////// - CollisionScene::CollisionScene(const std::string& root_name) - : root_name_(root_name) - { - } - - CollisionScene::~CollisionScene() - { - } - - void CollisionScene::updateCollisionObjects(const std::map>& objects) - { - fcl_objects_.resize(objects.size()); - int i=0; - for(const auto& object : objects) - { - std::shared_ptr new_object; - - const auto& cache_entry = fcl_cache_.find(object.first); - if(cache_entry == fcl_cache_.end()) - { - new_object = constructFclCollisionObject(object.second); - fcl_cache_[object.first] = new_object; - } - else - { - new_object = cache_entry->second; - } - fcl_objects_[i++] = new_object.get(); - } - } - - void CollisionScene::updateCollisionObjectTransforms() - { - for(fcl::CollisionObject* collision_object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(collision_object->getUserData()); - collision_object->setTransform(fcl_convert::KDL2fcl(element->Frame)); - collision_object->computeAABB(); - } - } - - // This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' - std::shared_ptr CollisionScene::constructFclCollisionObject(std::shared_ptr element) - { - // Maybe use cache here? - - shapes::ShapeConstPtr shape = element->Shape; - boost::shared_ptr geometry; - if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry - { - // handle cases individually - switch (shape->type) - { - case shapes::PLANE: - { - const shapes::Plane* p = static_cast(shape.get()); - geometry.reset(new fcl::Plane(p->a, p->b, p->c, p->d)); - } - break; - default: - break; - } - } - else - { - switch (shape->type) - { - case shapes::SPHERE: - { - const shapes::Sphere* s = static_cast(shape.get()); - geometry.reset(new fcl::Sphere(s->radius)); - } - break; - case shapes::BOX: - { - const shapes::Box* s = static_cast(shape.get()); - const double* size = s->size; - geometry.reset(new fcl::Box(size[0], size[1], size[2])); - } - break; - case shapes::CYLINDER: - { - const shapes::Cylinder* s = static_cast(shape.get()); - geometry.reset(new fcl::Cylinder(s->radius, s->length)); - } - break; - case shapes::CONE: - { - const shapes::Cone* s = static_cast(shape.get()); - geometry.reset(new fcl::Cone(s->radius, s->length)); - } - break; - case shapes::MESH: - { - fcl::BVHModel* g = new fcl::BVHModel(); - const shapes::Mesh* mesh = static_cast(shape.get()); - if (mesh->vertex_count > 0 && mesh->triangle_count > 0) - { - std::vector tri_indices(mesh->triangle_count); - for (unsigned int i = 0; i < mesh->triangle_count; ++i) - tri_indices[i] = - fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]); - - std::vector points(mesh->vertex_count); - for (unsigned int i = 0; i < mesh->vertex_count; ++i) - points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); - - g->beginModel(); - g->addSubModel(points, tri_indices); - g->endModel(); - } - geometry.reset(g); - } - break; - case shapes::OCTREE: - { - const shapes::OcTree* g = static_cast(shape.get()); - geometry.reset(new fcl::OcTree(g->octree)); - } - break; - default: - throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); - } - } - geometry->computeLocalAABB(); - geometry->setUserData(reinterpret_cast(element.get())); - std::shared_ptr ret(new fcl::CollisionObject(geometry)); - ret->setUserData(reinterpret_cast(element.get())); - - return ret; - } - - bool CollisionScene::isAllowedToCollide(fcl::CollisionObject* o1, fcl::CollisionObject* o2, bool self, CollisionScene* scene) - { - KinematicElement* e1 = reinterpret_cast(o1->getUserData()); - KinematicElement* e2 = reinterpret_cast(o2->getUserData()); - - bool isRobot1 = e1->isRobotLink || e1->ClosestRobotLink; - bool isRobot2 = e2->isRobotLink || e2->ClosestRobotLink; - // Don't check collisions between world objects - if(!isRobot1 && !isRobot2) return false; - // Skip self collisions if requested - if(isRobot1 && isRobot2 && !self) return false; - // Skip collisions between shapes within the same objects - if(e1->Parent==e2->Parent) return false; - // Skip collisions between bodies attached to the same object - if(e1->ClosestRobotLink&&e2->ClosestRobotLink&&e1->ClosestRobotLink==e2->ClosestRobotLink) return false; - - if(isRobot1 && isRobot2) - { - const std::string& name1 = e1->ClosestRobotLink?e1->ClosestRobotLink->Segment.getName():e1->Parent->Segment.getName(); - const std::string& name2 = e2->ClosestRobotLink?e2->ClosestRobotLink->Segment.getName():e2->Parent->Segment.getName(); - return scene->acm_.getAllowedCollision(name1, name2); - } - return true; - } - - bool CollisionScene::collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data) - { - CollisionData* data_ = reinterpret_cast(data); - - if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - - data_->Request.num_max_contacts = 1000; - data_->Result.clear(); - fcl::collide(o1,o2,data_->Request, data_->Result); - data_->Done = data_->Result.isCollision(); - return data_->Done; - } - - bool CollisionScene::collisionCallbackDistance(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist) - { - DistanceData* data_ = reinterpret_cast(data); - - if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - data_->Request.enable_nearest_points = false; - data_->Result.clear(); - fcl::distance(o1,o2,data_->Request, data_->Result); - int num_contacts = data_->Distance = std::min(data_->Distance, data_->Result.min_distance); - - CollisionProxy p; - p.e1 = reinterpret_cast(o1->getUserData()); - p.e2 = reinterpret_cast(o2->getUserData()); - - p.distance = data_->Result.min_distance; - fcl_convert::fcl2Eigen(data_->Result.nearest_points[0], p.contact1); - fcl_convert::fcl2Eigen(data_->Result.nearest_points[1], p.contact2); - p.normal1 = p.contact1.normalized(); - p.normal2 = p.contact2.normalized(); - p.distance = (p.contact1-p.contact2).norm(); - data_->Distance = std::min(data_->Distance, p.distance); - data_->Proxies.push_back(p); - - return false; - } - - bool CollisionScene::collisionCallbackContacts(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& dist) - { - ContactData* data_ = reinterpret_cast(data); - - if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - - data_->Request.enable_contact = true; - data_->Request.num_max_contacts = 1000; - data_->Result.clear(); - int num_contacts = fcl::collide(o1, o2, data_->Request, data_->Result); - CollisionProxy p; - p.e1 = reinterpret_cast(o1->getUserData()); - p.e2 = reinterpret_cast(o2->getUserData()); - - if(num_contacts>0) - { - p.distance = -data_->Result.getContact(0).penetration_depth; - for(int i=0; iResult.getContact(i); - if(p.distance>-contact.penetration_depth) - { - p.distance=-contact.penetration_depth; - if(reinterpret_cast(contact.o1->getUserData())==p.e1) - { - fcl_convert::fcl2Eigen(contact.pos, p.contact1); - fcl_convert::fcl2Eigen(contact.normal, p.normal1); - p.normal1.normalize(); - } - else if(reinterpret_cast(contact.o2->getUserData())==p.e2) - { - fcl_convert::fcl2Eigen(contact.pos, p.contact2); - fcl_convert::fcl2Eigen(contact.normal, p.normal2); - p.normal2.normalize(); - } - } - } - } - else - { - data_->DistanceRequest.enable_nearest_points = true; - data_->DistanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD; - fcl::distance(o1,o2,data_->DistanceRequest, data_->DistanceResult); - fcl_convert::fcl2Eigen(data_->DistanceResult.nearest_points[0], p.contact1); - fcl_convert::fcl2Eigen(data_->DistanceResult.nearest_points[1], p.contact2); - p.normal1 = p.contact1.normalized(); - p.normal2 = p.contact2.normalized(); - p.distance = (p.contact1-p.contact2).norm(); - } - data_->Distance = std::min(data_->Distance, p.distance); - data_->Proxies.push_back(p); - - return false; - } - - bool CollisionScene::isStateValid(bool self) - { - std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManager()); - manager->registerObjects(fcl_objects_); - CollisionData data(this); - data.Self = self; - manager->collide(&data, &CollisionScene::collisionCallback); - return !data.Result.isCollision(); - } - - std::vector CollisionScene::getCollisionDistance(bool self, bool computePenetrationDepth) - { - std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManager()); - manager->registerObjects(fcl_objects_); - if(computePenetrationDepth) - { - ContactData data(this); - data.Self = self; - manager->distance(&data, &CollisionScene::collisionCallbackContacts); - return data.Proxies; - } - else - { - DistanceData data(this); - data.Self = self; - manager->distance(&data, &CollisionScene::collisionCallbackDistance); - return data.Proxies; - } - } - - - Eigen::Vector3d CollisionScene::getTranslation(const std::string & name) - { - for(fcl::CollisionObject* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(element->Segment.getName()==name) - { - return Eigen::Map(element->Frame.p.data); - } - } - throw_pretty("Robot not found!");; - } - /////////////////////////////////////////////////////////////// /////////////////////// EXOTica Scene /////////////////////// /////////////////////////////////////////////////////////////// @@ -467,7 +98,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const << "/PlanningScene'"); } - collision_scene_.reset(new CollisionScene(kinematica_.getRootFrameName())); + collision_scene_ = Setup::createCollisionScene("CollisionSceneFCL"); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); AllowedCollisionMatrix acm; diff --git a/exotica/src/Setup.cpp b/exotica/src/Setup.cpp index 66e27e6ec1..86cc67a90c 100644 --- a/exotica/src/Setup.cpp +++ b/exotica/src/Setup.cpp @@ -58,6 +58,12 @@ namespace exotica { HIGHLIGHT(" '"< scenes = Instance()->scenes_.getDeclaredClasses(); + for(std::string s : scenes) + { + HIGHLIGHT(" '"< it, std::vector& ret) @@ -100,9 +106,10 @@ namespace exotica std::vector Setup::getSolvers() {return Instance()->solvers_.getDeclaredClasses();} std::vector Setup::getProblems() {return Instance()->problems_.getDeclaredClasses();} std::vector Setup::getMaps() {return Instance()->maps_.getDeclaredClasses();} + std::vector Setup::getCollisionScenes() {return Instance()->scenes_.getDeclaredClasses();} Setup::Setup() : solvers_("exotica","exotica::MotionSolver"), maps_("exotica","exotica::TaskMap"), - problems_(PlanningProblem_fac::Instance()) + problems_(PlanningProblem_fac::Instance()), scenes_("exotica","exotica::CollisionScene") { } diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp index 62482e98ad..edcbc52cc8 100644 --- a/exotica_python/src/Exotica.cpp +++ b/exotica_python/src/Exotica.cpp @@ -425,6 +425,7 @@ PYBIND11_MODULE(_pyexotica, module) setup.def_static("getSolvers", &Setup::getSolvers); setup.def_static("getProblems",&Setup::getProblems); setup.def_static("getMaps",&Setup::getMaps); + setup.def_static("getCollisionScenes",&Setup::getCollisionScenes); setup.def_static("createSolver", &createSolver, py::return_value_policy::take_ownership); setup.def_static("createMap",&createMap, py::return_value_policy::take_ownership); setup.def_static("createProblem",&createProblem, py::return_value_policy::take_ownership); From f5950cced7cd4a55ce80b2b5ad21610cbd2630ca Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Mon, 25 Sep 2017 05:19:23 +0100 Subject: [PATCH 12/23] Fixed normal orientation --- exotica/src/Scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 22b1d729f2..fa548a274b 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -172,8 +172,8 @@ namespace exotica { KDL::Vector c1 = proxies[i].e1->Frame*KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); KDL::Vector c2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); - KDL::Vector n1 = proxies[i].e1->Frame*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); - KDL::Vector n2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); + KDL::Vector n1 = proxies[i].e1->Frame.M*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); + KDL::Vector n2 = proxies[i].e2->Frame.M*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); tf::pointKDLToMsg(c1, ret.points[i*6]); tf::pointKDLToMsg(c2, ret.points[i*6+1]); tf::pointKDLToMsg(c1, ret.points[i*6+2]); From 49cd4440692211a93b0a282df418ddae11717b52 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Mon, 25 Sep 2017 05:20:02 +0100 Subject: [PATCH 13/23] Added FCL 0.6.0 collision scene --- .../collision_scene_fcl_latest/CMakeLists.txt | 39 +++ .../exotica_plugins.xml | 5 + .../CollisionSceneFCLLatest.h | 159 +++++++++ .../collision_scene_fcl_latest/package.xml | 17 + .../src/CollisionSceneFCLLatest.cpp | 301 ++++++++++++++++++ 5 files changed, 521 insertions(+) create mode 100644 exotations/collision_scene_fcl_latest/CMakeLists.txt create mode 100644 exotations/collision_scene_fcl_latest/exotica_plugins.xml create mode 100644 exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h create mode 100644 exotations/collision_scene_fcl_latest/package.xml create mode 100644 exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp diff --git a/exotations/collision_scene_fcl_latest/CMakeLists.txt b/exotations/collision_scene_fcl_latest/CMakeLists.txt new file mode 100644 index 0000000000..c624fb9934 --- /dev/null +++ b/exotations/collision_scene_fcl_latest/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 2.8.3) +project(collision_scene_fcl_latest) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + exotica + geometric_shapes +) + +find_package(fcl 0.6.0 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES collision_scene_fcl_latest + CATKIN_DEPENDS exotica geometric_shapes +) + +include_directories( + include + /usr/local/include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/CollisionSceneFCLLatest.cpp +) + +target_link_libraries(${PROJECT_NAME} + fcl +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(DIRECTORY include/ DESTINATION include) +install(FILES exotica_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/exotations/collision_scene_fcl_latest/exotica_plugins.xml b/exotations/collision_scene_fcl_latest/exotica_plugins.xml new file mode 100644 index 0000000000..1955884ff5 --- /dev/null +++ b/exotations/collision_scene_fcl_latest/exotica_plugins.xml @@ -0,0 +1,5 @@ + + + FCL collision scene + + diff --git a/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h new file mode 100644 index 0000000000..3b273bafe3 --- /dev/null +++ b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h @@ -0,0 +1,159 @@ +/* + * Author: Vladimir Ivan + * + * Copyright (c) 2017, University Of Edinburgh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef CollisionSceneFCLLatest_H +#define CollisionSceneFCLLatest_H + +#include +#include +#include +#include + +namespace exotica +{ +class CollisionSceneFCLLatest : public CollisionScene +{ +public: + + struct CollisionData + { + CollisionData(CollisionSceneFCLLatest* scene) : Scene(scene), Done(false), Self(true) {} + + fcl::CollisionRequestf Request; + fcl::CollisionResultf Result; + CollisionSceneFCLLatest* Scene; + bool Done; + bool Self; + }; + + struct DistanceData + { + DistanceData(CollisionSceneFCLLatest* scene) : Scene(scene), Self(true), Distance{1e300} {} + + fcl::DistanceRequestf Request; + fcl::DistanceResultf Result; + CollisionSceneFCLLatest* Scene; + std::vector Proxies; + double Distance; + bool Self; + }; + + CollisionSceneFCLLatest(); + + /** + * \brief Destructor + */ + virtual ~CollisionSceneFCLLatest(); + + static bool isAllowedToCollide(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, bool self, CollisionSceneFCLLatest* scene); + static bool collisionCallback(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data); + static bool collisionCallbackDistance(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data, float& dist); + + /** + * \brief Check if the whole robot is valid (collision only). + * @param self Indicate if self collision check is required. + * @return True, if the state is collision free.. + */ + virtual bool isStateValid(bool self = true); + + /// + /// \brief Computes collision distances. + /// \param self Indicate if self collision check is required. + /// \param computePenetrationDepth If set to true, accurate penetration depth is computed. + /// \return Collision proximity objectects for all colliding pairs of objects. + /// + virtual std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true); + + /** + * @brief Gets the collision world links. + * + * @return The collision world links. + */ + virtual std::vector getCollisionWorldLinks() + { + std::vector tmp; + for (fcl::CollisionObjectf* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(!element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; + } + + /** + * @brief Gets the collision robot links. + * + * @return The collision robot links. + */ + virtual std::vector getCollisionRobotLinks() + { + std::vector tmp; + for (fcl::CollisionObjectf* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; + } + + virtual Eigen::Vector3d getTranslation(const std::string & name); + + /// + /// \brief Creates the collision scene from kinematic elements. + /// \param objects Vector kinematic element pointers of collision objects. + /// + virtual void updateCollisionObjects(const std::map>& objects); + + /// + /// \brief Updates collision object transformations from the kinematic tree. + /// + virtual void updateCollisionObjectTransforms(); + +private: + + static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + + std::map> fcl_cache_; + + std::vector fcl_objects_; +}; + +typedef std::shared_ptr CollisionSceneFCLLatest_ptr; +} + +#endif // CollisionSceneFCLLatest_H diff --git a/exotations/collision_scene_fcl_latest/package.xml b/exotations/collision_scene_fcl_latest/package.xml new file mode 100644 index 0000000000..81c00dba5d --- /dev/null +++ b/exotations/collision_scene_fcl_latest/package.xml @@ -0,0 +1,17 @@ + + + collision_scene_fcl_latest + 0.0.0 + The collision checking using the FCL library. + Vladimir Ivan + + BSD + + catkin + exotica + geometric_shapes + + + + + diff --git a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp new file mode 100644 index 0000000000..f1331698ba --- /dev/null +++ b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp @@ -0,0 +1,301 @@ +/* + * Author: Vladimir Ivan + * + * Copyright (c) 2017, University Of Edinburgh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +REGISTER_COLLISION_SCENE_TYPE("CollisionSceneFCLLatest", exotica::CollisionSceneFCLLatest) + +namespace fcl_convert +{ +void fcl2Eigen(const fcl::Vector3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl(0); + eigen(1) = fcl(1); + eigen(2) = fcl(2); +} + +void fcl2Eigen(const fcl::Transform3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl.translation()(0); + eigen(1) = fcl.translation()(1); + eigen(2) = fcl.translation()(2); +} + +void fcl2EigenTranslation(const fcl::Vector3f & fcl, Eigen::Vector3d & eigen) +{ + eigen(0) = fcl(0); + eigen(1) = fcl(1); + eigen(2) = fcl(2); +} + +fcl::Transform3f KDL2fcl(const KDL::Frame& frame) +{ + fcl::Transform3f ret; + ret.matrix() = exotica::getFrame(frame).topLeftCorner<3,4>().cast(); + return ret; +} +} + +namespace exotica +{ +CollisionSceneFCLLatest::CollisionSceneFCLLatest() +{ + HIGHLIGHT("FCL version: "<>& objects) +{ + fcl_objects_.resize(objects.size()); + int i=0; + for(const auto& object : objects) + { + std::shared_ptr new_object; + + const auto& cache_entry = fcl_cache_.find(object.first); + if(cache_entry == fcl_cache_.end()) + { + new_object = constructFclCollisionObject(object.second); + fcl_cache_[object.first] = new_object; + } + else + { + new_object = cache_entry->second; + } + fcl_objects_[i++] = new_object.get(); + } +} + +void CollisionSceneFCLLatest::updateCollisionObjectTransforms() +{ + for(fcl::CollisionObjectf* collision_object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(collision_object->getUserData()); + collision_object->setTransform(fcl_convert::KDL2fcl(element->Frame)); + collision_object->computeAABB(); + } +} + +// This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' +std::shared_ptr CollisionSceneFCLLatest::constructFclCollisionObject(std::shared_ptr element) +{ + // Maybe use cache here? + + shapes::ShapeConstPtr shape = element->Shape; + std::shared_ptr geometry; + if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry + { + // handle cases individually + switch (shape->type) + { + case shapes::PLANE: + { + const shapes::Plane* p = static_cast(shape.get()); + geometry.reset(new fcl::Planef(p->a, p->b, p->c, p->d)); + } + break; + default: + break; + } + } + else + { + switch (shape->type) + { + case shapes::SPHERE: + { + const shapes::Sphere* s = static_cast(shape.get()); + geometry.reset(new fcl::Spheref(s->radius)); + } + break; + case shapes::BOX: + { + const shapes::Box* s = static_cast(shape.get()); + const double* size = s->size; + geometry.reset(new fcl::Boxf(size[0], size[1], size[2])); + } + break; + case shapes::CYLINDER: + { + const shapes::Cylinder* s = static_cast(shape.get()); + geometry.reset(new fcl::Cylinderf(s->radius, s->length)); + } + break; + case shapes::CONE: + { + const shapes::Cone* s = static_cast(shape.get()); + geometry.reset(new fcl::Conef(s->radius, s->length)); + } + break; + case shapes::MESH: + { + fcl::BVHModel* g = new fcl::BVHModel(); + const shapes::Mesh* mesh = static_cast(shape.get()); + if (mesh->vertex_count > 0 && mesh->triangle_count > 0) + { + std::vector tri_indices(mesh->triangle_count); + for (unsigned int i = 0; i < mesh->triangle_count; ++i) + tri_indices[i] = + fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]); + + std::vector points(mesh->vertex_count); + for (unsigned int i = 0; i < mesh->vertex_count; ++i) + points[i] = fcl::Vector3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); + + g->beginModel(); + g->addSubModel(points, tri_indices); + g->endModel(); + } + geometry.reset(g); + } + break; + case shapes::OCTREE: + { + const shapes::OcTree* g = static_cast(shape.get()); + geometry.reset(new fcl::OcTreef(to_std_ptr(g->octree))); + } + break; + default: + throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); + } + } + geometry->computeLocalAABB(); + geometry->setUserData(reinterpret_cast(element.get())); + std::shared_ptr ret(new fcl::CollisionObjectf(geometry)); + ret->setUserData(reinterpret_cast(element.get())); + + return ret; +} + +bool CollisionSceneFCLLatest::isAllowedToCollide(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, bool self, CollisionSceneFCLLatest* scene) +{ + KinematicElement* e1 = reinterpret_cast(o1->getUserData()); + KinematicElement* e2 = reinterpret_cast(o2->getUserData()); + + bool isRobot1 = e1->isRobotLink || e1->ClosestRobotLink; + bool isRobot2 = e2->isRobotLink || e2->ClosestRobotLink; + // Don't check collisions between world objects + if(!isRobot1 && !isRobot2) return false; + // Skip self collisions if requested + if(isRobot1 && isRobot2 && !self) return false; + // Skip collisions between shapes within the same objects + if(e1->Parent==e2->Parent) return false; + // Skip collisions between bodies attached to the same object + if(e1->ClosestRobotLink&&e2->ClosestRobotLink&&e1->ClosestRobotLink==e2->ClosestRobotLink) return false; + + if(isRobot1 && isRobot2) + { + const std::string& name1 = e1->ClosestRobotLink?e1->ClosestRobotLink->Segment.getName():e1->Parent->Segment.getName(); + const std::string& name2 = e2->ClosestRobotLink?e2->ClosestRobotLink->Segment.getName():e2->Parent->Segment.getName(); + return scene->acm_.getAllowedCollision(name1, name2); + } + return true; +} + +bool CollisionSceneFCLLatest::collisionCallback(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data) +{ + CollisionData* data_ = reinterpret_cast(data); + + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + + data_->Request.num_max_contacts = 1000; + data_->Result.clear(); + fcl::collide(o1,o2,data_->Request, data_->Result); + data_->Done = data_->Result.isCollision(); + return data_->Done; +} + +bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data, float& dist) +{ + DistanceData* data_ = reinterpret_cast(data); + + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + data_->Request.enable_nearest_points = true; + data_->Request.enable_signed_distance = true; + data_->Result.clear(); + fcl::distance(o1,o2,data_->Request, data_->Result); + data_->Distance = std::min(data_->Distance, (double)data_->Result.min_distance); + + CollisionProxy p; + p.e1 = reinterpret_cast(o1->getUserData()); + p.e2 = reinterpret_cast(o2->getUserData()); + + p.distance = data_->Result.min_distance; + fcl_convert::fcl2Eigen(data_->Result.nearest_points[0], p.contact1); + fcl_convert::fcl2Eigen(data_->Result.nearest_points[1], p.contact2); + if(p.contact1(0)!=p.contact1(0)) return false; + p.normal1 = p.contact1.normalized(); + p.normal2 = p.contact2.normalized(); + data_->Distance = std::min(data_->Distance, p.distance); + data_->Proxies.push_back(p); + //HIGHLIGHT(p.e1->Segment.getName()<<" - "<Segment.getName()<<": "< manager(new fcl::DynamicAABBTreeCollisionManagerf()); + manager->registerObjects(fcl_objects_); + CollisionData data(this); + data.Self = self; + manager->collide(&data, &CollisionSceneFCLLatest::collisionCallback); + return !data.Result.isCollision(); +} + +std::vector CollisionSceneFCLLatest::getCollisionDistance(bool self, bool computePenetrationDepth) +{ + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManagerf()); + manager->registerObjects(fcl_objects_); + DistanceData data(this); + data.Self = self; + manager->distance(&data, &CollisionSceneFCLLatest::collisionCallbackDistance); + return data.Proxies; +} + +Eigen::Vector3d CollisionSceneFCLLatest::getTranslation(const std::string & name) +{ + for(fcl::CollisionObjectf* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->Segment.getName()==name) + { + return Eigen::Map(element->Frame.p.data); + } + } + throw_pretty("Robot not found!");; +} +} From c9b020f60e2dcc73348fc4e778320d4cabb0c211 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 13:37:31 +0100 Subject: [PATCH 14/23] Switched to using double precision --- .../CollisionSceneFCLLatest.h | 24 ++-- .../src/CollisionSceneFCLLatest.cpp | 104 ++++++++++++------ 2 files changed, 81 insertions(+), 47 deletions(-) diff --git a/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h index 3b273bafe3..b4e8030cb0 100644 --- a/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h +++ b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h @@ -48,8 +48,8 @@ class CollisionSceneFCLLatest : public CollisionScene { CollisionData(CollisionSceneFCLLatest* scene) : Scene(scene), Done(false), Self(true) {} - fcl::CollisionRequestf Request; - fcl::CollisionResultf Result; + fcl::CollisionRequestd Request; + fcl::CollisionResultd Result; CollisionSceneFCLLatest* Scene; bool Done; bool Self; @@ -59,8 +59,8 @@ class CollisionSceneFCLLatest : public CollisionScene { DistanceData(CollisionSceneFCLLatest* scene) : Scene(scene), Self(true), Distance{1e300} {} - fcl::DistanceRequestf Request; - fcl::DistanceResultf Result; + fcl::DistanceRequestd Request; + fcl::DistanceResultd Result; CollisionSceneFCLLatest* Scene; std::vector Proxies; double Distance; @@ -74,9 +74,9 @@ class CollisionSceneFCLLatest : public CollisionScene */ virtual ~CollisionSceneFCLLatest(); - static bool isAllowedToCollide(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, bool self, CollisionSceneFCLLatest* scene); - static bool collisionCallback(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data); - static bool collisionCallbackDistance(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data, float& dist); + static bool isAllowedToCollide(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, bool self, CollisionSceneFCLLatest* scene); + static bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data); + static bool collisionCallbackDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& dist); /** * \brief Check if the whole robot is valid (collision only). @@ -101,7 +101,7 @@ class CollisionSceneFCLLatest : public CollisionScene virtual std::vector getCollisionWorldLinks() { std::vector tmp; - for (fcl::CollisionObjectf* object : fcl_objects_) + for (fcl::CollisionObjectd* object : fcl_objects_) { KinematicElement* element = reinterpret_cast(object->getUserData()); if(!element->ClosestRobotLink) @@ -120,7 +120,7 @@ class CollisionSceneFCLLatest : public CollisionScene virtual std::vector getCollisionRobotLinks() { std::vector tmp; - for (fcl::CollisionObjectf* object : fcl_objects_) + for (fcl::CollisionObjectd* object : fcl_objects_) { KinematicElement* element = reinterpret_cast(object->getUserData()); if(element->ClosestRobotLink) @@ -146,11 +146,11 @@ class CollisionSceneFCLLatest : public CollisionScene private: - static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); - std::map> fcl_cache_; + std::map> fcl_cache_; - std::vector fcl_objects_; + std::vector fcl_objects_; }; typedef std::shared_ptr CollisionSceneFCLLatest_ptr; diff --git a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp index f1331698ba..0326d446da 100644 --- a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp +++ b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp @@ -32,37 +32,38 @@ #include #include +#include REGISTER_COLLISION_SCENE_TYPE("CollisionSceneFCLLatest", exotica::CollisionSceneFCLLatest) namespace fcl_convert { -void fcl2Eigen(const fcl::Vector3f & fcl, Eigen::Vector3d & eigen) +void fcl2Eigen(const fcl::Vector3d & fcl, Eigen::Vector3d & eigen) { eigen(0) = fcl(0); eigen(1) = fcl(1); eigen(2) = fcl(2); } -void fcl2Eigen(const fcl::Transform3f & fcl, Eigen::Vector3d & eigen) +void fcl2Eigen(const fcl::Transform3d & fcl, Eigen::Vector3d & eigen) { eigen(0) = fcl.translation()(0); eigen(1) = fcl.translation()(1); eigen(2) = fcl.translation()(2); } -void fcl2EigenTranslation(const fcl::Vector3f & fcl, Eigen::Vector3d & eigen) +void fcl2EigenTranslation(const fcl::Vector3d & fcl, Eigen::Vector3d & eigen) { eigen(0) = fcl(0); eigen(1) = fcl(1); eigen(2) = fcl(2); } -fcl::Transform3f KDL2fcl(const KDL::Frame& frame) +fcl::Transform3d KDL2fcl(const KDL::Frame& frame) { - fcl::Transform3f ret; - ret.matrix() = exotica::getFrame(frame).topLeftCorner<3,4>().cast(); - return ret; + Eigen::Affine3d ret; + tf::transformKDLToEigen(frame, ret); + return fcl::Transform3d(ret); } } @@ -83,7 +84,7 @@ void CollisionSceneFCLLatest::updateCollisionObjects(const std::map new_object; + std::shared_ptr new_object; const auto& cache_entry = fcl_cache_.find(object.first); if(cache_entry == fcl_cache_.end()) @@ -101,7 +102,7 @@ void CollisionSceneFCLLatest::updateCollisionObjects(const std::map(collision_object->getUserData()); collision_object->setTransform(fcl_convert::KDL2fcl(element->Frame)); @@ -110,12 +111,12 @@ void CollisionSceneFCLLatest::updateCollisionObjectTransforms() } // This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' -std::shared_ptr CollisionSceneFCLLatest::constructFclCollisionObject(std::shared_ptr element) +std::shared_ptr CollisionSceneFCLLatest::constructFclCollisionObject(std::shared_ptr element) { // Maybe use cache here? shapes::ShapeConstPtr shape = element->Shape; - std::shared_ptr geometry; + std::shared_ptr geometry; if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry { // handle cases individually @@ -124,7 +125,7 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl case shapes::PLANE: { const shapes::Plane* p = static_cast(shape.get()); - geometry.reset(new fcl::Planef(p->a, p->b, p->c, p->d)); + geometry.reset(new fcl::Planed(p->a, p->b, p->c, p->d)); } break; default: @@ -138,31 +139,31 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl case shapes::SPHERE: { const shapes::Sphere* s = static_cast(shape.get()); - geometry.reset(new fcl::Spheref(s->radius)); + geometry.reset(new fcl::Sphered(s->radius)); } break; case shapes::BOX: { const shapes::Box* s = static_cast(shape.get()); const double* size = s->size; - geometry.reset(new fcl::Boxf(size[0], size[1], size[2])); + geometry.reset(new fcl::Boxd(size[0], size[1], size[2])); } break; case shapes::CYLINDER: { const shapes::Cylinder* s = static_cast(shape.get()); - geometry.reset(new fcl::Cylinderf(s->radius, s->length)); + geometry.reset(new fcl::Cylinderd(s->radius, s->length)); } break; case shapes::CONE: { const shapes::Cone* s = static_cast(shape.get()); - geometry.reset(new fcl::Conef(s->radius, s->length)); + geometry.reset(new fcl::Coned(s->radius, s->length)); } break; case shapes::MESH: { - fcl::BVHModel* g = new fcl::BVHModel(); + fcl::BVHModel* g = new fcl::BVHModel(); const shapes::Mesh* mesh = static_cast(shape.get()); if (mesh->vertex_count > 0 && mesh->triangle_count > 0) { @@ -171,9 +172,9 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]); - std::vector points(mesh->vertex_count); + std::vector points(mesh->vertex_count); for (unsigned int i = 0; i < mesh->vertex_count; ++i) - points[i] = fcl::Vector3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); + points[i] = fcl::Vector3d(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]); g->beginModel(); g->addSubModel(points, tri_indices); @@ -185,7 +186,7 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl case shapes::OCTREE: { const shapes::OcTree* g = static_cast(shape.get()); - geometry.reset(new fcl::OcTreef(to_std_ptr(g->octree))); + geometry.reset(new fcl::OcTreed(to_std_ptr(g->octree))); } break; default: @@ -194,13 +195,13 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl } geometry->computeLocalAABB(); geometry->setUserData(reinterpret_cast(element.get())); - std::shared_ptr ret(new fcl::CollisionObjectf(geometry)); + std::shared_ptr ret(new fcl::CollisionObjectd(geometry)); ret->setUserData(reinterpret_cast(element.get())); return ret; } -bool CollisionSceneFCLLatest::isAllowedToCollide(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, bool self, CollisionSceneFCLLatest* scene) +bool CollisionSceneFCLLatest::isAllowedToCollide(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, bool self, CollisionSceneFCLLatest* scene) { KinematicElement* e1 = reinterpret_cast(o1->getUserData()); KinematicElement* e2 = reinterpret_cast(o2->getUserData()); @@ -225,7 +226,7 @@ bool CollisionSceneFCLLatest::isAllowedToCollide(fcl::CollisionObjectf* o1, fcl: return true; } -bool CollisionSceneFCLLatest::collisionCallback(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data) +bool CollisionSceneFCLLatest::collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data) { CollisionData* data_ = reinterpret_cast(data); @@ -233,18 +234,20 @@ bool CollisionSceneFCLLatest::collisionCallback(fcl::CollisionObjectf* o1, fcl:: data_->Request.num_max_contacts = 1000; data_->Result.clear(); - fcl::collide(o1,o2,data_->Request, data_->Result); + fcl::collide(o1, o2, data_->Request, data_->Result); data_->Done = data_->Result.isCollision(); return data_->Done; } -bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectf* o1, fcl::CollisionObjectf* o2, void* data, float& dist) +bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& dist) { DistanceData* data_ = reinterpret_cast(data); if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; data_->Request.enable_nearest_points = true; data_->Request.enable_signed_distance = true; + // GST_LIBCCD produces incorrect contacts. Probably due to incompatible version of libccd. + data_->Request.gjk_solver_type = fcl::GST_INDEP; data_->Result.clear(); fcl::distance(o1,o2,data_->Request, data_->Result); data_->Distance = std::min(data_->Distance, (double)data_->Result.min_distance); @@ -254,21 +257,52 @@ bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectf* o p.e2 = reinterpret_cast(o2->getUserData()); p.distance = data_->Result.min_distance; - fcl_convert::fcl2Eigen(data_->Result.nearest_points[0], p.contact1); - fcl_convert::fcl2Eigen(data_->Result.nearest_points[1], p.contact2); - if(p.contact1(0)!=p.contact1(0)) return false; - p.normal1 = p.contact1.normalized(); - p.normal2 = p.contact2.normalized(); + if(true || p.distance>0) + { + KDL::Vector c1 = (p.distance>0?p.e1->Frame:KDL::Frame())*KDL::Vector(data_->Result.nearest_points[0](0), data_->Result.nearest_points[0](1), data_->Result.nearest_points[0](2)); + KDL::Vector c2 = (p.distance>0?p.e2->Frame:KDL::Frame())*KDL::Vector(data_->Result.nearest_points[1](0), data_->Result.nearest_points[1](1), data_->Result.nearest_points[1](2)); + KDL::Vector n1 = c2-c1; + KDL::Vector n2 = c1-c2; + n1.Normalize(); + n2.Normalize(); + tf::vectorKDLToEigen(c1, p.contact1); + tf::vectorKDLToEigen(c2, p.contact2); + tf::vectorKDLToEigen(n1, p.normal1); + tf::vectorKDLToEigen(n2, p.normal2); + } + else + { + fcl::CollisionRequestd req; + fcl::CollisionResultd res; + req.enable_contact = true; + req.num_max_contacts = 2; + req.gjk_solver_type = fcl::GST_INDEP; + { + res.clear(); + fcl::collide(o1, o2, req, res); + const fcl::Contactd& contact = res.getContact(0); + p.contact1 = contact.pos; + p.normal1 = contact.normal; + p.distance = contact.penetration_depth; + } + { + res.clear(); + fcl::collide(o2, o1, req, res); + const fcl::Contactd& contact = res.getContact(0); + p.contact2 = contact.pos; + p.normal2 = contact.normal; + p.distance = contact.penetration_depth; + } + + } data_->Distance = std::min(data_->Distance, p.distance); data_->Proxies.push_back(p); - //HIGHLIGHT(p.e1->Segment.getName()<<" - "<Segment.getName()<<": "< manager(new fcl::DynamicAABBTreeCollisionManagerf()); + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManagerd()); manager->registerObjects(fcl_objects_); CollisionData data(this); data.Self = self; @@ -278,7 +312,7 @@ bool CollisionSceneFCLLatest::isStateValid(bool self) std::vector CollisionSceneFCLLatest::getCollisionDistance(bool self, bool computePenetrationDepth) { - std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManagerf()); + std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManagerd()); manager->registerObjects(fcl_objects_); DistanceData data(this); data.Self = self; @@ -288,7 +322,7 @@ std::vector CollisionSceneFCLLatest::getCollisionDistance(bool s Eigen::Vector3d CollisionSceneFCLLatest::getTranslation(const std::string & name) { - for(fcl::CollisionObjectf* object : fcl_objects_) + for(fcl::CollisionObjectd* object : fcl_objects_) { KinematicElement* element = reinterpret_cast(object->getUserData()); if(element->Segment.getName()==name) From b42ae76b64724b6189477bfa0ea8fb5727eea678 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 13:38:20 +0100 Subject: [PATCH 15/23] switched to using fcl_catkin --- exotations/collision_scene_fcl_latest/CMakeLists.txt | 8 +++----- exotations/collision_scene_fcl_latest/package.xml | 1 + 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/exotations/collision_scene_fcl_latest/CMakeLists.txt b/exotations/collision_scene_fcl_latest/CMakeLists.txt index c624fb9934..7751d17c5f 100644 --- a/exotations/collision_scene_fcl_latest/CMakeLists.txt +++ b/exotations/collision_scene_fcl_latest/CMakeLists.txt @@ -6,19 +6,17 @@ add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS exotica geometric_shapes + fcl_catkin ) -find_package(fcl 0.6.0 REQUIRED) - catkin_package( INCLUDE_DIRS include LIBRARIES collision_scene_fcl_latest - CATKIN_DEPENDS exotica geometric_shapes + CATKIN_DEPENDS exotica geometric_shapes fcl_catkin ) include_directories( include - /usr/local/include ${catkin_INCLUDE_DIRS} ) @@ -27,7 +25,7 @@ add_library(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} - fcl + ${catkin_LIBRARIES} ) install(TARGETS ${PROJECT_NAME} diff --git a/exotations/collision_scene_fcl_latest/package.xml b/exotations/collision_scene_fcl_latest/package.xml index 81c00dba5d..8b642aab4c 100644 --- a/exotations/collision_scene_fcl_latest/package.xml +++ b/exotations/collision_scene_fcl_latest/package.xml @@ -10,6 +10,7 @@ catkin exotica geometric_shapes + fcl_catkin From fcfadec928288183e5b4457f733a74dd4653bed2 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 13:38:54 +0100 Subject: [PATCH 16/23] Changed proxy publish frame transformation --- exotica/src/Scene.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index fa548a274b..46e678579f 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -170,10 +170,10 @@ namespace exotica std_msgs::ColorRGBA colliding = getColor(1,0,0); for(int i=0; iFrame*KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); - KDL::Vector c2 = proxies[i].e2->Frame*KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); - KDL::Vector n1 = proxies[i].e1->Frame.M*KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); - KDL::Vector n2 = proxies[i].e2->Frame.M*KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); + KDL::Vector c1 = KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2)); + KDL::Vector c2 = KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2)); + KDL::Vector n1 = KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2)); + KDL::Vector n2 = KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2)); tf::pointKDLToMsg(c1, ret.points[i*6]); tf::pointKDLToMsg(c2, ret.points[i*6+1]); tf::pointKDLToMsg(c1, ret.points[i*6+2]); From 01a121472ba57f9eb0e4708ab81d031a0bd7f568 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 26 Sep 2017 16:17:42 +0100 Subject: [PATCH 17/23] Add fcl_catkin submodule --- .gitmodules | 3 +++ fcl_catkin | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 fcl_catkin diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000..e0e21d27f4 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "fcl_catkin"] + path = fcl_catkin + url = https://github.com/wxmerkt/fcl_catkin.git diff --git a/fcl_catkin b/fcl_catkin new file mode 160000 index 0000000000..79522b6245 --- /dev/null +++ b/fcl_catkin @@ -0,0 +1 @@ +Subproject commit 79522b6245a5d3af7a4b10a1dae3b2e7f6578534 From 323003c018b43a2ec7a68a9c96c1c337d89bff12 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 16:37:20 +0100 Subject: [PATCH 18/23] Changed to using shape centres when in collision --- .../src/CollisionSceneFCLLatest.cpp | 40 +++++++------------ 1 file changed, 14 insertions(+), 26 deletions(-) diff --git a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp index 0326d446da..33ecac601d 100644 --- a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp +++ b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp @@ -250,17 +250,16 @@ bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o data_->Request.gjk_solver_type = fcl::GST_INDEP; data_->Result.clear(); fcl::distance(o1,o2,data_->Request, data_->Result); - data_->Distance = std::min(data_->Distance, (double)data_->Result.min_distance); CollisionProxy p; p.e1 = reinterpret_cast(o1->getUserData()); p.e2 = reinterpret_cast(o2->getUserData()); p.distance = data_->Result.min_distance; - if(true || p.distance>0) + if(p.distance>0) { - KDL::Vector c1 = (p.distance>0?p.e1->Frame:KDL::Frame())*KDL::Vector(data_->Result.nearest_points[0](0), data_->Result.nearest_points[0](1), data_->Result.nearest_points[0](2)); - KDL::Vector c2 = (p.distance>0?p.e2->Frame:KDL::Frame())*KDL::Vector(data_->Result.nearest_points[1](0), data_->Result.nearest_points[1](1), data_->Result.nearest_points[1](2)); + KDL::Vector c1 = p.e1->Frame*KDL::Vector(data_->Result.nearest_points[0](0), data_->Result.nearest_points[0](1), data_->Result.nearest_points[0](2)); + KDL::Vector c2 = p.e2->Frame*KDL::Vector(data_->Result.nearest_points[1](0), data_->Result.nearest_points[1](1), data_->Result.nearest_points[1](2)); KDL::Vector n1 = c2-c1; KDL::Vector n2 = c1-c2; n1.Normalize(); @@ -272,28 +271,17 @@ bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o } else { - fcl::CollisionRequestd req; - fcl::CollisionResultd res; - req.enable_contact = true; - req.num_max_contacts = 2; - req.gjk_solver_type = fcl::GST_INDEP; - { - res.clear(); - fcl::collide(o1, o2, req, res); - const fcl::Contactd& contact = res.getContact(0); - p.contact1 = contact.pos; - p.normal1 = contact.normal; - p.distance = contact.penetration_depth; - } - { - res.clear(); - fcl::collide(o2, o1, req, res); - const fcl::Contactd& contact = res.getContact(0); - p.contact2 = contact.pos; - p.normal2 = contact.normal; - p.distance = contact.penetration_depth; - } - + // Contact points are not reliable (FCL bug), use shape centres instead. + KDL::Vector c1 = p.e1->Frame.p; + KDL::Vector c2 = p.e2->Frame.p; + KDL::Vector n1 = c2-c1; + KDL::Vector n2 = c1-c2; + n1.Normalize(); + n2.Normalize(); + tf::vectorKDLToEigen(c1, p.contact1); + tf::vectorKDLToEigen(c2, p.contact2); + tf::vectorKDLToEigen(n1, p.normal1); + tf::vectorKDLToEigen(n2, p.normal2); } data_->Distance = std::min(data_->Distance, p.distance); data_->Proxies.push_back(p); From 113c858347d552269fc954ce7a93f0a1f377c38c Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 16:38:27 +0100 Subject: [PATCH 19/23] Changed proxy markers --- exotica/src/Scene.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 46e678579f..21529c83a3 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -164,7 +164,7 @@ namespace exotica ret.points.resize(proxies.size()*6); ret.colors.resize(proxies.size()*6); ret.scale.x = 0.005; - double normalLength = 0.05; + double normalLength = 0.01; std_msgs::ColorRGBA normal = getColor(0.8,0.8,0.8); std_msgs::ColorRGBA far = getColor(0.5,0.5,0.5); std_msgs::ColorRGBA colliding = getColor(1,0,0); @@ -180,8 +180,7 @@ namespace exotica tf::pointKDLToMsg(c1+n1*normalLength, ret.points[i*6+3]); tf::pointKDLToMsg(c2, ret.points[i*6+4]); tf::pointKDLToMsg(c2+n2*normalLength, ret.points[i*6+5]); - ret.colors[i*6] = proxies[i].distance>0?far:colliding; - ret.colors[i*6+1] = proxies[i].distance>0?far:colliding; + ret.colors[i*6] = ret.colors[i*6+1] = proxies[i].distance>0?far:colliding; ret.colors[i*6+2]=ret.colors[i*6+3]=ret.colors[i*6+4]=ret.colors[i*6+5]=normal; } return ret; From b8939429c7158c1a3e0aad113d157674845657d9 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 16:38:55 +0100 Subject: [PATCH 20/23] Added collision scene parameter into the initializer --- exotica/init/Scene.in | 1 + exotica/src/Scene.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/exotica/init/Scene.in b/exotica/init/Scene.in index 0e16a557eb..d45bb09fdd 100644 --- a/exotica/init/Scene.in +++ b/exotica/init/Scene.in @@ -3,3 +3,4 @@ Required std::string JointGroup; Optional std::string RobotDescription = "robot_description"; Optional std::string URDF = ""; Optional std::string SRDF = ""; +Optional std::string CollisionScene = "CollisionSceneFCL"; diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp index 21529c83a3..111f40993c 100644 --- a/exotica/src/Scene.cpp +++ b/exotica/src/Scene.cpp @@ -98,7 +98,7 @@ namespace exotica << "/PlanningScene'"); } - collision_scene_ = Setup::createCollisionScene("CollisionSceneFCL"); + collision_scene_ = Setup::createCollisionScene(init.CollisionScene); collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap()); AllowedCollisionMatrix acm; From 8ef1a5e3f474162ce8533ef02e4b60efbcac3401 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Tue, 26 Sep 2017 16:39:36 +0100 Subject: [PATCH 21/23] Added collision distance example, updated attach example --- .../resources/configs/example_distance.xml | 24 +++ .../configs/example_manipulate_ompl.xml | 1 - exotica/resources/rviz-distance.rviz | 187 ++++++++++++++++++ .../resources/scenes/example_distance.scene | 9 + .../resources/scenes/example_manipulate.scene | 14 ++ .../launch/collisionDistance.launch | 13 ++ exotica_python/scripts/example_distance.py | 20 ++ 7 files changed, 267 insertions(+), 1 deletion(-) create mode 100644 exotica/resources/configs/example_distance.xml create mode 100644 exotica/resources/rviz-distance.rviz create mode 100644 exotica/resources/scenes/example_distance.scene create mode 100644 exotica_python/launch/collisionDistance.launch create mode 100755 exotica_python/scripts/example_distance.py diff --git a/exotica/resources/configs/example_distance.xml b/exotica/resources/configs/example_distance.xml new file mode 100644 index 0000000000..907d59b77d --- /dev/null +++ b/exotica/resources/configs/example_distance.xml @@ -0,0 +1,24 @@ + + + + + RRTConnect + OMPLImpSolver + + + + + + + Sampling + arm + {exotica}/resources/robots/lwr_simplified.urdf + {exotica}/resources/robots/lwr_simplified.srdf + CollisionSceneFCLLatest + + + + 0 0 0 0 0 0 0 + + + diff --git a/exotica/resources/configs/example_manipulate_ompl.xml b/exotica/resources/configs/example_manipulate_ompl.xml index b5979b6abe..42910ab242 100644 --- a/exotica/resources/configs/example_manipulate_ompl.xml +++ b/exotica/resources/configs/example_manipulate_ompl.xml @@ -10,7 +10,6 @@ - Sampling arm {exotica}/resources/robots/lwr_simplified.urdf diff --git a/exotica/resources/rviz-distance.rviz b/exotica/resources/rviz-distance.rviz new file mode 100644 index 0000000000..6e8e6ad179 --- /dev/null +++ b/exotica/resources/rviz-distance.rviz @@ -0,0 +1,187 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Proximity1/Namespaces1 + Splitter Ratio: 0.589404 + Tree Height: 1751 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lwr_arm_7_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: exotica + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /exotica/CollisionShapes + Name: SceneObjects + Namespaces: + CollisionObjects: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /exotica/CollisionProxies + Name: Proximity + Namespaces: + Proxies: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: exotica/world_frame + Frame Rate: 100 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.40737 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.220149 + Y: -0.14348 + Z: 0.658301 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.484798 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.64594 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2112 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000035200000787fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df000002e20000029ffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000787000000fe00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000016300000787fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000787000000e200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e9e0000005cfc0100000002fb0000000800540069006d0065010000000000000e9e0000055100fffffffb0000000800540069006d00650100000000000004500000000000000000000009dd0000078700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3742 + X: 98 + Y: 48 diff --git a/exotica/resources/scenes/example_distance.scene b/exotica/resources/scenes/example_distance.scene new file mode 100644 index 0000000000..7d20520ae5 --- /dev/null +++ b/exotica/resources/scenes/example_distance.scene @@ -0,0 +1,9 @@ +(ExampleScene)+ +* Obstacle +1 +box +0.1 0.2 1.0 +0.25 0 0.55 +0 0 0.968912 -0.247404 +0 0 0 0 +. diff --git a/exotica/resources/scenes/example_manipulate.scene b/exotica/resources/scenes/example_manipulate.scene index a9f21d4e95..f246898f4b 100644 --- a/exotica/resources/scenes/example_manipulate.scene +++ b/exotica/resources/scenes/example_manipulate.scene @@ -6,4 +6,18 @@ box 10 0 0.9 0 0 0 1 0 0 0 0 +* Obstacle +1 +box +0.1 0.2 1.0 +0.25 0 0.55 +0 0 0 1 +0 0 0 0 +* Obstacle2 +1 +box +0.4 1.0 0.02 +0.5 0 0.7 +0 0 0 1 +0 0 0 0 . diff --git a/exotica_python/launch/collisionDistance.launch b/exotica_python/launch/collisionDistance.launch new file mode 100644 index 0000000000..9320990001 --- /dev/null +++ b/exotica_python/launch/collisionDistance.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/exotica_python/scripts/example_distance.py b/exotica_python/scripts/example_distance.py new file mode 100755 index 0000000000..0f6826c6cf --- /dev/null +++ b/exotica_python/scripts/example_distance.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python + +import pyexotica as exo +from pyexotica.publish_trajectory import * +import math + +exo.Setup.initRos() +ompl=exo.Setup.loadSolver('{exotica}/resources/configs/example_distance.xml') +ompl.getProblem().getScene().loadSceneFile('{exotica}/resources/scenes/example_distance.scene') +sc=ompl.getProblem().getScene() + +dt=0.01 +t=0.0 +while not is_shutdown(): + sc.Update([math.sin(t/2.0)*0.5]*7) + p=sc.getCollisionDistance(False) + sc.getSolver().publishFrames() + sc.publishProxies(sc.getCollisionDistance(False)) + t=t+dt + sleep(dt) From 1a3bfdb8bd62298229a389c4362e77fdb7841caf Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 26 Sep 2017 20:31:37 +0100 Subject: [PATCH 22/23] Travis: add lccd-debs ppa - Workaround for https://github.com/travis-ci/apt-source-whitelist/issues/229 --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 9a80210895..b7b09f94f0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,7 @@ env: - secure: "hq/S+hFDH44PBWcskCANx2Qli+KE8n8b5a5e+IR5T81j6b/NW8WW9Tx02vQjS8KFHcVNHFd9UcCCiKZrsFqs/GbGjBcRknGTGfslesBzZ2B/eXwR1B6dcG4dnN9xLIZw3j8s+i0JoMn1d3Hx/LJgmXVLdgKp0zrLeYDJCkwJ084nAfe3hgizyqL1JMn9xWIyklpR+qi0KVEeu6aV3w18HpDz87NRG42IRiBtLynTPl39y3H+UQNUKtI9gkIb6fuOQY7e5bK9CcJInQl6RDIWK93BPIzPMrDfzpfej93cP/eyvKbBdmzx6hJRI2qrrEXzY7Aj3gB6W26cQU1A9BoWrRD+pbqC6/EcO8/8LKfyXNj4RWiKkrnUAMMZ48LgLQULVLSEP8PB/KoH0EKB2cm98Tu7DimYUznsyVXfkpMMBj0CDG0Nbi3QiS7j0AcN1ngJkIdn8A64pRHKX/waDLdOPGqnezK5UUqhWrqNwoed8emCxsBvGkbUNL/fsnMtdYThzLvW2b6cMHa1O3brhjVpDLbvieFs/0OBMz02lSlVNE0H6ADJJYiPCF1QHi//D+Dqt1B9ZgwMv3ZfD7xTSdxYFAlcLa1YRKeiDdFMXmz3lxl0IPZLk2Ub4/ScbPzXtidsAnffZoiXZBhGCcHywy/hHLcT/mO3a61hiiwgLujkPcY=" before_install: - sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test +- sudo add-apt-repository -y ppa:libccd-debs/ppa - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update -qq From ac204179bbaed9eae60818e7ce6c7117eb87184b Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Fri, 29 Sep 2017 16:27:29 +0100 Subject: [PATCH 23/23] PR fixes: - Removed CollisionData.Done - Separated narrow phace routine - Added collision/distance check overloads taking object names - Fixed typose, comments, and removed depricated XML tags --- .../collision_scene_fcl/CollisionSceneFCL.h | 35 +--- .../src/CollisionSceneFCL.cpp | 119 ++++++++--- .../CollisionSceneFCLLatest.h | 37 +--- .../src/CollisionSceneFCLLatest.cpp | 198 +++++++++++------- exotica/include/exotica/CollisionScene.h | 23 +- exotica/include/exotica/KinematicTree.h | 2 +- .../resources/configs/example_distance.xml | 1 - .../configs/example_manipulate_ompl.xml | 1 - .../resources/configs/ompl_solver_demo.xml | 2 - exotica_python/src/Exotica.cpp | 4 +- 10 files changed, 249 insertions(+), 173 deletions(-) diff --git a/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h b/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h index 26dc4469db..31a270ff83 100644 --- a/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h +++ b/exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h @@ -52,12 +52,11 @@ class CollisionSceneFCL : public CollisionScene struct CollisionData { - CollisionData(CollisionSceneFCL* scene) : Scene(scene), Done(false), Self(true) {} + CollisionData(CollisionSceneFCL* scene) : Scene(scene), Self(true) {} fcl::CollisionRequest Request; fcl::CollisionResult Result; CollisionSceneFCL* Scene; - bool Done; bool Self; }; @@ -79,44 +78,19 @@ class CollisionSceneFCL : public CollisionScene * @return True, if the state is collision free.. */ virtual bool isStateValid(bool self = true); + virtual bool isCollisionFree(const std::string& o1, const std::string& o2); /** * @brief Gets the collision world links. - * * @return The collision world links. */ - virtual std::vector getCollisionWorldLinks() - { - std::vector tmp; - for (fcl::CollisionObject* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(!element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } + virtual std::vector getCollisionWorldLinks(); /** * @brief Gets the collision robot links. - * * @return The collision robot links. */ - virtual std::vector getCollisionRobotLinks() - { - std::vector tmp; - for (fcl::CollisionObject* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } + virtual std::vector getCollisionRobotLinks(); virtual Eigen::Vector3d getTranslation(const std::string & name); @@ -134,6 +108,7 @@ class CollisionSceneFCL : public CollisionScene private: static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + static void checkCollision(fcl::CollisionObject* o1, fcl::CollisionObject* o2, CollisionData* data); std::map> fcl_cache_; diff --git a/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp b/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp index 09fc3fc1e9..9981134415 100644 --- a/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp +++ b/exotations/collision_scene_fcl/src/CollisionSceneFCL.cpp @@ -107,57 +107,47 @@ void CollisionSceneFCL::updateCollisionObjectTransforms() } // This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' +// https://github.com/ros-planning/moveit/blob/indigo-devel/moveit_core/collision_detection_fcl/src/collision_common.cpp#L512 std::shared_ptr CollisionSceneFCL::constructFclCollisionObject(std::shared_ptr element) { // Maybe use cache here? shapes::ShapeConstPtr shape = element->Shape; boost::shared_ptr geometry; - if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry + switch (shape->type) { - // handle cases individually - switch (shape->type) - { - case shapes::PLANE: + case shapes::PLANE: { const shapes::Plane* p = static_cast(shape.get()); geometry.reset(new fcl::Plane(p->a, p->b, p->c, p->d)); } - break; - default: - break; - } - } - else - { - switch (shape->type) - { - case shapes::SPHERE: + break; + case shapes::SPHERE: { const shapes::Sphere* s = static_cast(shape.get()); geometry.reset(new fcl::Sphere(s->radius)); } - break; - case shapes::BOX: + break; + case shapes::BOX: { const shapes::Box* s = static_cast(shape.get()); const double* size = s->size; geometry.reset(new fcl::Box(size[0], size[1], size[2])); } - break; - case shapes::CYLINDER: + break; + case shapes::CYLINDER: { const shapes::Cylinder* s = static_cast(shape.get()); geometry.reset(new fcl::Cylinder(s->radius, s->length)); } - break; - case shapes::CONE: + break; + case shapes::CONE: { const shapes::Cone* s = static_cast(shape.get()); geometry.reset(new fcl::Cone(s->radius, s->length)); } - break; - case shapes::MESH: + break; + case shapes::MESH: { fcl::BVHModel* g = new fcl::BVHModel(); const shapes::Mesh* mesh = static_cast(shape.get()); @@ -178,16 +168,15 @@ std::shared_ptr CollisionSceneFCL::constructFclCollisionOb } geometry.reset(g); } - break; - case shapes::OCTREE: + break; + case shapes::OCTREE: { const shapes::OcTree* g = static_cast(shape.get()); geometry.reset(new fcl::OcTree(g->octree)); } - break; - default: - throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); - } + break; + default: + throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); } geometry->computeLocalAABB(); geometry->setUserData(reinterpret_cast(element.get())); @@ -222,17 +211,21 @@ bool CollisionSceneFCL::isAllowedToCollide(fcl::CollisionObject* o1, fcl::Collis return true; } +void CollisionSceneFCL::checkCollision(fcl::CollisionObject* o1, fcl::CollisionObject* o2, CollisionData* data) +{ + data->Request.num_max_contacts = 1000; + data->Result.clear(); + fcl::collide(o1,o2,data->Request, data->Result); +} + bool CollisionSceneFCL::collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data) { CollisionData* data_ = reinterpret_cast(data); if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - data_->Request.num_max_contacts = 1000; - data_->Result.clear(); - fcl::collide(o1,o2,data_->Request, data_->Result); - data_->Done = data_->Result.isCollision(); - return data_->Done; + checkCollision(o1, o2, data_); + return data_->Result.isCollision(); } bool CollisionSceneFCL::isStateValid(bool self) @@ -245,6 +238,30 @@ bool CollisionSceneFCL::isStateValid(bool self) return !data.Result.isCollision(); } +bool CollisionSceneFCL::isCollisionFree(const std::string& o1, const std::string& o2) +{ + std::vector shapes1; + std::vector shapes2; + for(fcl::CollisionObject* o : fcl_objects_) + { + KinematicElement* e = reinterpret_cast(o->getUserData()); + if(e->Segment.getName()==o1 || e->Parent->Segment.getName()==o1) shapes1.push_back(o); + if(e->Segment.getName()==o2 || e->Parent->Segment.getName()==o2) shapes2.push_back(o); + } + if(shapes1.size()==0) throw_pretty("Can't find object '"< CollisionSceneFCL::getCollisionWorldLinks() +{ + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(!element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; +} + +/** + * @brief Gets the collision robot links. + * + * @return The collision robot links. + */ +std::vector CollisionSceneFCL::getCollisionRobotLinks() +{ + std::vector tmp; + for (fcl::CollisionObject* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; +} + } diff --git a/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h index b4e8030cb0..7993de1dc3 100644 --- a/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h +++ b/exotations/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h @@ -46,12 +46,11 @@ class CollisionSceneFCLLatest : public CollisionScene struct CollisionData { - CollisionData(CollisionSceneFCLLatest* scene) : Scene(scene), Done(false), Self(true) {} + CollisionData(CollisionSceneFCLLatest* scene) : Scene(scene), Self(true) {} fcl::CollisionRequestd Request; fcl::CollisionResultd Result; CollisionSceneFCLLatest* Scene; - bool Done; bool Self; }; @@ -84,6 +83,7 @@ class CollisionSceneFCLLatest : public CollisionScene * @return True, if the state is collision free.. */ virtual bool isStateValid(bool self = true); + virtual bool isCollisionFree(const std::string& o1, const std::string& o2); /// /// \brief Computes collision distances. @@ -91,45 +91,22 @@ class CollisionSceneFCLLatest : public CollisionScene /// \param computePenetrationDepth If set to true, accurate penetration depth is computed. /// \return Collision proximity objectects for all colliding pairs of objects. /// - virtual std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true); + virtual std::vector getCollisionDistance(bool self); + virtual std::vector getCollisionDistance(const std::string& o1, const std::string& o2); /** * @brief Gets the collision world links. * * @return The collision world links. */ - virtual std::vector getCollisionWorldLinks() - { - std::vector tmp; - for (fcl::CollisionObjectd* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(!element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } + virtual std::vector getCollisionWorldLinks(); /** * @brief Gets the collision robot links. * * @return The collision robot links. */ - virtual std::vector getCollisionRobotLinks() - { - std::vector tmp; - for (fcl::CollisionObjectd* object : fcl_objects_) - { - KinematicElement* element = reinterpret_cast(object->getUserData()); - if(element->ClosestRobotLink) - { - tmp.push_back(element->Segment.getName()); - } - } - return tmp; - } + virtual std::vector getCollisionRobotLinks(); virtual Eigen::Vector3d getTranslation(const std::string & name); @@ -147,6 +124,8 @@ class CollisionSceneFCLLatest : public CollisionScene private: static std::shared_ptr constructFclCollisionObject(std::shared_ptr element); + static void checkCollision(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, CollisionData* data); + static void computeDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, DistanceData* data); std::map> fcl_cache_; diff --git a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp index 33ecac601d..d63b110a6b 100644 --- a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp +++ b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp @@ -38,27 +38,6 @@ REGISTER_COLLISION_SCENE_TYPE("CollisionSceneFCLLatest", exotica::CollisionScene namespace fcl_convert { -void fcl2Eigen(const fcl::Vector3d & fcl, Eigen::Vector3d & eigen) -{ - eigen(0) = fcl(0); - eigen(1) = fcl(1); - eigen(2) = fcl(2); -} - -void fcl2Eigen(const fcl::Transform3d & fcl, Eigen::Vector3d & eigen) -{ - eigen(0) = fcl.translation()(0); - eigen(1) = fcl.translation()(1); - eigen(2) = fcl.translation()(2); -} - -void fcl2EigenTranslation(const fcl::Vector3d & fcl, Eigen::Vector3d & eigen) -{ - eigen(0) = fcl(0); - eigen(1) = fcl(1); - eigen(2) = fcl(2); -} - fcl::Transform3d KDL2fcl(const KDL::Frame& frame) { Eigen::Affine3d ret; @@ -111,57 +90,47 @@ void CollisionSceneFCLLatest::updateCollisionObjectTransforms() } // This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp' +// https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_core/collision_detection_fcl/src/collision_common.cpp#L520 std::shared_ptr CollisionSceneFCLLatest::constructFclCollisionObject(std::shared_ptr element) { // Maybe use cache here? shapes::ShapeConstPtr shape = element->Shape; std::shared_ptr geometry; - if (shape->type == shapes::PLANE) // shapes that directly produce CollisionGeometry + switch (shape->type) { - // handle cases individually - switch (shape->type) - { - case shapes::PLANE: + case shapes::PLANE: { const shapes::Plane* p = static_cast(shape.get()); geometry.reset(new fcl::Planed(p->a, p->b, p->c, p->d)); } - break; - default: - break; - } - } - else - { - switch (shape->type) - { - case shapes::SPHERE: + break; + case shapes::SPHERE: { const shapes::Sphere* s = static_cast(shape.get()); geometry.reset(new fcl::Sphered(s->radius)); } - break; - case shapes::BOX: + break; + case shapes::BOX: { const shapes::Box* s = static_cast(shape.get()); const double* size = s->size; geometry.reset(new fcl::Boxd(size[0], size[1], size[2])); } - break; - case shapes::CYLINDER: + break; + case shapes::CYLINDER: { const shapes::Cylinder* s = static_cast(shape.get()); geometry.reset(new fcl::Cylinderd(s->radius, s->length)); } - break; - case shapes::CONE: + break; + case shapes::CONE: { const shapes::Cone* s = static_cast(shape.get()); geometry.reset(new fcl::Coned(s->radius, s->length)); } - break; - case shapes::MESH: + break; + case shapes::MESH: { fcl::BVHModel* g = new fcl::BVHModel(); const shapes::Mesh* mesh = static_cast(shape.get()); @@ -182,16 +151,15 @@ std::shared_ptr CollisionSceneFCLLatest::constructFclColl } geometry.reset(g); } - break; - case shapes::OCTREE: + break; + case shapes::OCTREE: { const shapes::OcTree* g = static_cast(shape.get()); geometry.reset(new fcl::OcTreed(to_std_ptr(g->octree))); } - break; - default: - throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); - } + break; + default: + throw_pretty("This shape type ("<<((int)shape->type)<<") is not supported using FCL yet"); } geometry->computeLocalAABB(); geometry->setUserData(reinterpret_cast(element.get())); @@ -226,40 +194,41 @@ bool CollisionSceneFCLLatest::isAllowedToCollide(fcl::CollisionObjectd* o1, fcl: return true; } +void CollisionSceneFCLLatest::checkCollision(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, CollisionData* data) +{ + data->Request.num_max_contacts = 1000; + data->Result.clear(); + fcl::collide(o1,o2,data->Request, data->Result); +} + bool CollisionSceneFCLLatest::collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data) { CollisionData* data_ = reinterpret_cast(data); if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - data_->Request.num_max_contacts = 1000; - data_->Result.clear(); - fcl::collide(o1, o2, data_->Request, data_->Result); - data_->Done = data_->Result.isCollision(); - return data_->Done; + checkCollision(o1, o2, data_); + return data_->Result.isCollision(); } -bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& dist) +void CollisionSceneFCLLatest::computeDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, DistanceData* data) { - DistanceData* data_ = reinterpret_cast(data); - - if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; - data_->Request.enable_nearest_points = true; - data_->Request.enable_signed_distance = true; + data->Request.enable_nearest_points = true; + data->Request.enable_signed_distance = true; // Added in FCL 0.6.0 // GST_LIBCCD produces incorrect contacts. Probably due to incompatible version of libccd. - data_->Request.gjk_solver_type = fcl::GST_INDEP; - data_->Result.clear(); - fcl::distance(o1,o2,data_->Request, data_->Result); + data->Request.gjk_solver_type = fcl::GST_INDEP; + data->Result.clear(); + fcl::distance(o1,o2,data->Request, data->Result); CollisionProxy p; p.e1 = reinterpret_cast(o1->getUserData()); p.e2 = reinterpret_cast(o2->getUserData()); - p.distance = data_->Result.min_distance; + p.distance = data->Result.min_distance; if(p.distance>0) { - KDL::Vector c1 = p.e1->Frame*KDL::Vector(data_->Result.nearest_points[0](0), data_->Result.nearest_points[0](1), data_->Result.nearest_points[0](2)); - KDL::Vector c2 = p.e2->Frame*KDL::Vector(data_->Result.nearest_points[1](0), data_->Result.nearest_points[1](1), data_->Result.nearest_points[1](2)); + KDL::Vector c1 = p.e1->Frame*KDL::Vector(data->Result.nearest_points[0](0), data->Result.nearest_points[0](1), data->Result.nearest_points[0](2)); + KDL::Vector c2 = p.e2->Frame*KDL::Vector(data->Result.nearest_points[1](0), data->Result.nearest_points[1](1), data->Result.nearest_points[1](2)); KDL::Vector n1 = c2-c1; KDL::Vector n2 = c1-c2; n1.Normalize(); @@ -283,8 +252,16 @@ bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o tf::vectorKDLToEigen(n1, p.normal1); tf::vectorKDLToEigen(n2, p.normal2); } - data_->Distance = std::min(data_->Distance, p.distance); - data_->Proxies.push_back(p); + data->Distance = std::min(data->Distance, p.distance); + data->Proxies.push_back(p); +} + +bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& dist) +{ + DistanceData* data_ = reinterpret_cast(data); + + if(!isAllowedToCollide(o1, o2, data_->Self, data_->Scene)) return false; + computeDistance(o1, o2, data_); return false; } @@ -298,7 +275,31 @@ bool CollisionSceneFCLLatest::isStateValid(bool self) return !data.Result.isCollision(); } -std::vector CollisionSceneFCLLatest::getCollisionDistance(bool self, bool computePenetrationDepth) +bool CollisionSceneFCLLatest::isCollisionFree(const std::string& o1, const std::string& o2) +{ + std::vector shapes1; + std::vector shapes2; + for(fcl::CollisionObjectd* o : fcl_objects_) + { + KinematicElement* e = reinterpret_cast(o->getUserData()); + if(e->Segment.getName()==o1 || e->Parent->Segment.getName()==o1) shapes1.push_back(o); + if(e->Segment.getName()==o2 || e->Parent->Segment.getName()==o2) shapes2.push_back(o); + } + if(shapes1.size()==0) throw_pretty("Can't find object '"< CollisionSceneFCLLatest::getCollisionDistance(bool self) { std::shared_ptr manager(new fcl::DynamicAABBTreeCollisionManagerd()); manager->registerObjects(fcl_objects_); @@ -308,6 +309,29 @@ std::vector CollisionSceneFCLLatest::getCollisionDistance(bool s return data.Proxies; } +std::vector CollisionSceneFCLLatest::getCollisionDistance(const std::string& o1, const std::string& o2) +{ + std::vector shapes1; + std::vector shapes2; + for(fcl::CollisionObjectd* o : fcl_objects_) + { + KinematicElement* e = reinterpret_cast(o->getUserData()); + if(e->Segment.getName()==o1 || e->Parent->Segment.getName()==o1) shapes1.push_back(o); + if(e->Segment.getName()==o2 || e->Parent->Segment.getName()==o2) shapes2.push_back(o); + } + if(shapes1.size()==0) throw_pretty("Can't find object '"< CollisionSceneFCLLatest::getCollisionWorldLinks() +{ + std::vector tmp; + for (fcl::CollisionObjectd* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(!element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; +} + +/** + * @brief Gets the collision robot links. + * + * @return The collision robot links. + */ +std::vector CollisionSceneFCLLatest::getCollisionRobotLinks() +{ + std::vector tmp; + for (fcl::CollisionObjectd* object : fcl_objects_) + { + KinematicElement* element = reinterpret_cast(object->getUserData()); + if(element->ClosestRobotLink) + { + tmp.push_back(element->Segment.getName()); + } + } + return tmp; +} + } diff --git a/exotica/include/exotica/CollisionScene.h b/exotica/include/exotica/CollisionScene.h index 3e39133b91..53de7cc367 100644 --- a/exotica/include/exotica/CollisionScene.h +++ b/exotica/include/exotica/CollisionScene.h @@ -80,19 +80,34 @@ class CollisionScene : public Uncopyable virtual ~CollisionScene() {} /** - * \brief Check if the whole robot is valid (collision only). + * \brief Checks if the whole robot is valid (collision only). * @param self Indicate if self collision check is required. * @return True, if the state is collision free.. */ virtual bool isStateValid(bool self = true) = 0; + /// + /// @brief Checks if two objects are in collision. + /// @param o1 Name of object 1. + /// @param o2 Name of object 2. + /// @return True is the two objects are not colliding. + /// + virtual bool isCollisionFree(const std::string& o1, const std::string& o2) {throw_pretty("Not implemented!");} + /// /// \brief Computes collision distances. /// \param self Indicate if self collision check is required. - /// \param computePenetrationDepth If set to true, accurate penetration depth is computed. - /// \return Collision proximity objectects for all colliding pairs of objects. + /// \return Collision proximity objects for all colliding pairs of shapes. + /// + virtual std::vector getCollisionDistance(bool self) {throw_pretty("Not implemented!");} + + /// + /// \brief Computes collision distances between two objects. + /// \param o1 Name of object 1. + /// \param o2 Name of object 2. + /// \return Vector of proximity objects. /// - virtual std::vector getCollisionDistance(bool self, bool computePenetrationDepth = true) {throw_pretty("Not implemented!");} + virtual std::vector getCollisionDistance(const std::string& o1, const std::string& o2) {throw_pretty("Not implemented!");} /** * @brief Gets the collision world links. diff --git a/exotica/include/exotica/KinematicTree.h b/exotica/include/exotica/KinematicTree.h index 38e2292d1d..350a658f53 100644 --- a/exotica/include/exotica/KinematicTree.h +++ b/exotica/include/exotica/KinematicTree.h @@ -87,7 +87,7 @@ namespace exotica { public: KinematicFrameRequest(); - KinematicFrameRequest(std::string frameALinkName, KDL::Frame frameAOffset = KDL::Frame(), std::string frameBLinkName = "", KDL::Frame frameBOffset = KDL::Frame()); + KinematicFrameRequest(std::string frameALinkName, KDL::Frame frameAOffset = KDL::Frame(), std::string frameBLinkName = "", KDL::Frame frameBOffset = KDL::Frame()); std::string FrameALinkName; KDL::Frame FrameAOffset; std::string FrameBLinkName; diff --git a/exotica/resources/configs/example_distance.xml b/exotica/resources/configs/example_distance.xml index 907d59b77d..d5de0400b6 100644 --- a/exotica/resources/configs/example_distance.xml +++ b/exotica/resources/configs/example_distance.xml @@ -10,7 +10,6 @@ - Sampling arm {exotica}/resources/robots/lwr_simplified.urdf {exotica}/resources/robots/lwr_simplified.srdf diff --git a/exotica/resources/configs/example_manipulate_ompl.xml b/exotica/resources/configs/example_manipulate_ompl.xml index 42910ab242..0d7436946f 100644 --- a/exotica/resources/configs/example_manipulate_ompl.xml +++ b/exotica/resources/configs/example_manipulate_ompl.xml @@ -10,7 +10,6 @@ - Sampling arm {exotica}/resources/robots/lwr_simplified.urdf {exotica}/resources/robots/lwr_simplified.srdf diff --git a/exotica/resources/configs/ompl_solver_demo.xml b/exotica/resources/configs/ompl_solver_demo.xml index d1a454ca62..2fd3e406f4 100644 --- a/exotica/resources/configs/ompl_solver_demo.xml +++ b/exotica/resources/configs/ompl_solver_demo.xml @@ -10,8 +10,6 @@ - - Sampling arm {exotica}/resources/robots/lwr_simplified.urdf {exotica}/resources/robots/lwr_simplified.srdf diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp index edcbc52cc8..f08a101af2 100644 --- a/exotica_python/src/Exotica.cpp +++ b/exotica_python/src/Exotica.cpp @@ -588,7 +588,9 @@ PYBIND11_MODULE(_pyexotica, module) scene.def("getScene", &Scene::getScene); scene.def("cleanScene", &Scene::cleanScene); scene.def("isStateValid", [](Scene* instance, bool self) {return instance->getCollisionScene()->isStateValid(self);}, py::arg("self")=true); - scene.def("getCollisionDistance", [](Scene* instance, bool self, bool distances) {return instance->getCollisionScene()->getCollisionDistance(self, distances);}, py::arg("self")=true, py::arg("computeAccurateDistances")=true); + scene.def("isCollisionFree", [](Scene* instance, const std::string& o1, const std::string& o2) {return instance->getCollisionScene()->isCollisionFree(o1, o2);}); + scene.def("getCollisionDistance", [](Scene* instance, bool self) {return instance->getCollisionScene()->getCollisionDistance(self);}, py::arg("self")=true); + scene.def("getCollisionDistance", [](Scene* instance, const std::string& o1, const std::string& o2) {return instance->getCollisionScene()->getCollisionDistance(o1, o2);}, py::arg("Object1"), py::arg("Object2")); scene.def("updateWorld", &Scene::updateWorld); scene.def("getCollisionRobotLinks", [](Scene* instance) {return instance->getCollisionScene()->getCollisionRobotLinks();}); scene.def("getCollisionWorldLinks", [](Scene* instance) {return instance->getCollisionScene()->getCollisionWorldLinks();});