Merge pull request #125 from ipab-slmc/vi-collisionscene

Replaced collision checking from MoveIt by a direct interface to FCL

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "fcl_catkin"]
+	path = fcl_catkin
+	url = https://github.com/wxmerkt/fcl_catkin.git

.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

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})

exotations/collision_scene_fcl/exotica_plugins.xml

@@ -0,0 +1,5 @@
+<library path="lib/libcollision_scene_fcl">
+  <class name="exotica/CollisionSceneFCL" type="exotica::CollisionSceneFCL" base_class_type="exotica::CollisionScene">
+    <description>FCL collision scene</description>
+  </class>
+</library>

exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h

@@ -0,0 +1,121 @@
+/*
+ *      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 <exotica/CollisionScene.h>
+#include <fcl/broadphase/broadphase.h>
+#include <fcl/narrowphase/narrowphase.h>
+#include <fcl/collision.h>
+#include <fcl/distance.h>
+#include <fcl/BVH/BVH_model.h>
+#include <fcl/shape/geometric_shapes.h>
+#include <fcl/octree.h>
+#include <geometric_shapes/mesh_operations.h>
+#include <geometric_shapes/shape_operations.h>
+
+namespace exotica
+{
+class CollisionSceneFCL : public CollisionScene
+{
+public:
+
+    struct CollisionData
+    {
+        CollisionData(CollisionSceneFCL* scene) : Scene(scene), Self(true) {}
+
+        fcl::CollisionRequest Request;
+        fcl::CollisionResult Result;
+        CollisionSceneFCL* Scene;
+        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);
+    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<std::string> getCollisionWorldLinks();
+
+    /**
+       * @brief      Gets the collision robot links.
+       * @return     The collision robot links.
+       */
+    virtual std::vector<std::string> getCollisionRobotLinks();
+
+    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<std::string, std::shared_ptr<KinematicElement>>& objects);
+
+    ///
+    /// \brief Updates collision object transformations from the kinematic tree.
+    ///
+    virtual void updateCollisionObjectTransforms();
+
+private:
+
+    static std::shared_ptr<fcl::CollisionObject> constructFclCollisionObject(std::shared_ptr<KinematicElement> element);
+    static void checkCollision(fcl::CollisionObject* o1, fcl::CollisionObject* o2, CollisionData* data);
+
+    std::map<std::string, std::shared_ptr<fcl::CollisionObject>> fcl_cache_;
+
+    std::vector<fcl::CollisionObject*> fcl_objects_;
+};
+
+typedef std::shared_ptr<CollisionSceneFCL> CollisionSceneFCL_ptr;
+}
+
+#endif // COLLISIONSCENEFCL_H

exotations/collision_scene_fcl/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>collision_scene_fcl</name>
+  <version>0.0.0</version>
+  <description>The collision checking using the FCL library.</description>
+  <maintainer email="vivan@ed.ac.uk">Vladimir Ivan</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>exotica</depend>
+  <depend>geometric_shapes</depend>
+  <depend>fcl</depend>
+
+  <export>
+    <exotica plugin="${prefix}/exotica_plugins.xml" />
+  </export>
+</package>