feat: load collision mesh

.gitmodules

@@ -4,3 +4,9 @@
 [submodule "third_party/pybind11"]
 	path = third_party/pybind11
 	url = https://github.com/pybind/pybind11.git
+[submodule "third_party/assimp"]
+	path = third_party/assimp
+	url = https://github.com/assimp/assimp.git
+[submodule "third_party/fcl"]
+	path = third_party/fcl
+	url = https://github.com/flexible-collision-library/fcl.git

CMakeLists.txt

@@ -10,14 +10,28 @@ set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -Wall -fPIC")
 add_subdirectory("urdf_parser")
 include_directories("third_party" "urdf_parser/include")
 
+set(BUILD_SHARED_LIBS OFF)
+set(ASSIMP_BUILD_TESTS OFF)
+set(ASSIMP_INSTALL OFF)
+add_subdirectory("third_party/assimp")
+
+set(FCL_STATIC_LIBRARY ON)
+set(FCL_WITH_OCTOMAP OFF)
+set(BUILD_TESTING OFF)
+add_subdirectory("third_party/fcl")
+
 find_package(Eigen3 REQUIRED)
 ADD_DEFINITIONS(-DEIGEN_NO_DEBUG)
 include_directories(${EIGEN3_INCLUDE_DIR})
 
-set(TINYFK_SRC src/tinyfk.cpp src/kinematics.cpp)
 include_directories(include/)
+
+add_library(loadmesh STATIC src/mesh.cpp)
+target_link_libraries(loadmesh assimp fcl)
+
+set(TINYFK_SRC src/tinyfk.cpp src/kinematics.cpp)
 add_library(tinyfk STATIC ${TINYFK_SRC})
-target_link_libraries(tinyfk urdfdom_model)
+target_link_libraries(tinyfk loadmesh urdfdom_model)
 
 # testing
 option(BUILD_TEST "build google test" OFF)

bench/bench_tinyfk.cpp

@@ -51,8 +51,10 @@ void benchmark_fk(KinematicModel &kin, size_t n_iter,
 }
 
 int main() {
-  const std::string urdf_file = "../data/fetch.urdf";
+  const std::string urdf_file =
+      "/home/h-ishida/.skrobot/fetch_description/fetch.urdf";
   const auto urdf_string = load_urdf(urdf_file);
+  std::cout << urdf_string << std::endl;
   auto kin = KinematicModel(urdf_string);
 
   const size_t N = 100000;

include/mesh.hpp

@@ -0,0 +1,10 @@
+#include <fcl/broadphase/broadphase_collision_manager.h>
+#include <fcl/narrowphase/collision.h>
+#include <fcl/narrowphase/distance.h>
+
+namespace tinyfk {
+
+std::shared_ptr<fcl::BVHModel<fcl::OBBRSSf>>
+load_fcl_geometry(const std::string &urdf_path, const std::string &mesh_path);
+
+}

include/tinyfk.hpp

@@ -9,10 +9,12 @@ tinyfk: https://github.com/HiroIshida/tinyfk
 
 #include "urdf_model/joint.h"
 #include "urdf_model/pose.h"
+#include "urdf_model/types.h"
 #include "urdf_parser/urdf_parser.h"
 #include <Eigen/Core> // slow compile...
 #include <array>
 #include <assert.h>
+#include <fcl/narrowphase/collision.h>
 #include <fstream>
 #include <iostream>
 #include <stack>
@@ -55,6 +57,7 @@ class KinematicModel {
   std::vector<urdf::LinkSharedPtr> links_;
   std::unordered_map<std::string, int> link_ids_;
   std::vector<urdf::LinkSharedPtr> com_dummy_links_;
+  std::vector<std::shared_ptr<fcl::CollisionGeometryf>> coll_geoms_;
 
   std::vector<urdf::JointSharedPtr> joints_;
   std::unordered_map<std::string, int> joint_ids_;
@@ -70,7 +73,7 @@ class KinematicModel {
   mutable SizedCache<urdf::Pose> transform_cache_;
 
 public: // functions
-  KinematicModel(const std::string &xml_string);
+  KinematicModel(const std::string &urdf_path);
 
   virtual ~KinematicModel() {}
 

src/mesh.cpp

@@ -0,0 +1,111 @@
+#include "mesh.hpp"
+#include <array>
+#include <assimp/Importer.hpp>
+#include <assimp/postprocess.h>
+#include <assimp/scene.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace tinyfk {
+
+struct Trimesh {
+  std::vector<fcl::Vector3f> vertices;
+  std::vector<fcl::Triangle> faces;
+};
+
+Trimesh merge_trimeshes(const std::vector<Trimesh> &trimeshes) {
+  Trimesh trimesh_new;
+  size_t idx_head = 0;
+  for (const auto &trimesh : trimeshes) {
+    for (const auto &v : trimesh.vertices) {
+      trimesh_new.vertices.push_back(v);
+    }
+    for (const auto &f : trimesh.faces) {
+      trimesh_new.faces.emplace_back(f[0] + idx_head, f[1] + idx_head,
+                                     f[2] + idx_head);
+    }
+    idx_head += trimesh.vertices.size();
+  }
+  return trimesh_new;
+}
+
+Trimesh load_trimesh(const std::string &file_path) {
+  Assimp::Importer importer;
+  const aiScene *scene = importer.ReadFile(file_path, 0);
+
+  if (scene == nullptr) {
+    throw std::runtime_error("scene is nullptr");
+  }
+  if (scene->mNumMeshes == 0) {
+    throw std::runtime_error("scene has 0 mesh");
+  }
+
+  std::vector<Trimesh> trimeshes;
+
+  for (size_t i = 0; i < scene->mNumMeshes; ++i) {
+    const auto mesh = scene->mMeshes[i];
+
+    auto trimesh = Trimesh();
+
+    for (size_t j = 0; j < mesh->mNumVertices; ++j) {
+      double x = mesh->mVertices[j].x;
+      double y = mesh->mVertices[j].y;
+      double z = mesh->mVertices[j].z;
+      trimesh.vertices.emplace_back(x, y, z);
+    }
+
+    for (size_t j = 0; j < mesh->mNumFaces; ++j) {
+      size_t idx1 = mesh->mFaces[j].mIndices[0] + 1;
+      size_t idx2 = mesh->mFaces[j].mIndices[1] + 1;
+      size_t idx3 = mesh->mFaces[j].mIndices[2] + 1;
+      trimesh.faces.emplace_back(idx1, idx2, idx3);
+    }
+    trimeshes.push_back(trimesh);
+  }
+
+  if (trimeshes.size() == 1) {
+    return trimeshes.at(0);
+  }
+
+  const auto trimesh_merged = merge_trimeshes(trimeshes);
+  return trimesh_merged;
+}
+
+std::string resolve_file_path(const std::string &urdf_file_path,
+                              const std::string &mesh_file_path) {
+  bool start_with_package = (mesh_file_path.rfind("package://", 0) == 0);
+  std::string raw_path = mesh_file_path;
+  if (start_with_package) {
+    raw_path.erase(0, 10);
+  }
+
+  std::string base_directory =
+      urdf_file_path.substr(0, urdf_file_path.find_last_of("/\\"));
+
+  std::vector<std::string> prepath_candidates = {"./", "../", "../../"};
+  for (auto &prepath : prepath_candidates) {
+    auto fullpath = base_directory + "/" + prepath + raw_path;
+    std::ifstream test_exist(fullpath);
+    if (test_exist) {
+      return fullpath;
+    }
+  }
+  throw std::runtime_error("cannot resolve file path");
+}
+
+std::shared_ptr<fcl::BVHModel<fcl::OBBRSSf>>
+load_fcl_geometry(const std::string &urdf_path, const std::string &mesh_path) {
+  const auto mesh = load_trimesh(resolve_file_path(urdf_path, mesh_path));
+  std::shared_ptr<fcl::BVHModel<fcl::OBBRSSf>> geom =
+      std::make_shared<fcl::BVHModel<fcl::OBBRSSf>>();
+  geom->beginModel();
+  geom->addSubModel(mesh.vertices, mesh.faces);
+  geom->endModel();
+  return geom;
+}
+
+} // namespace tinyfk

src/tinyfk.cpp

@@ -5,15 +5,21 @@ tinyfk: https://github.com/HiroIshida/tinyfk
 */
 
 #include "tinyfk.hpp"
+#include "mesh.hpp"
+#include "urdf_model/link.h"
 #include "urdf_model/pose.h"
 #include <Eigen/Geometry>
 #include <cmath>
+#include <fcl/geometry/collision_geometry.h>
+#include <fcl/geometry/shape/box.h>
 #include <fstream>
+#include <memory>
 #include <stdexcept>
 
 namespace tinyfk {
 
-KinematicModel::KinematicModel(const std::string &xml_string) {
+KinematicModel::KinematicModel(const std::string &urdf_path) {
+  const auto xml_string = load_urdf(urdf_path);
   if (xml_string.empty()) {
     throw std::runtime_error("xml string is empty");
   }
@@ -34,6 +40,36 @@ KinematicModel::KinematicModel(const std::string &xml_string) {
   }
   size_t N_link = lid; // starting from 0 and finally ++ increment, so it'S ok
 
+  // loading meshes
+  std::vector<std::shared_ptr<fcl::CollisionGeometryf>> coll_geoms;
+  for (const auto &link : links) {
+    std::shared_ptr<fcl::CollisionGeometryf> coll_geom;
+    if (link->collision != nullptr) {
+      const auto urdf_geometry = link->collision->geometry;
+      if (urdf_geometry->type == urdf::Geometry::BOX) {
+        const auto box = urdf::dynamic_pointer_cast<urdf::Box>(urdf_geometry);
+        coll_geom =
+            std::make_shared<fcl::Boxf>(box->dim.x, box->dim.y, box->dim.z);
+
+      } else if (urdf_geometry->type == urdf::Geometry::SPHERE) {
+        const auto sphere =
+            urdf::dynamic_pointer_cast<urdf::Sphere>(urdf_geometry);
+        coll_geom = std::make_shared<fcl::Spheref>(sphere->radius);
+
+      } else if (urdf_geometry->type == urdf::Geometry::CYLINDER) {
+        const auto cylinder =
+            urdf::dynamic_pointer_cast<urdf::Cylinder>(urdf_geometry);
+        coll_geom = std::make_shared<fcl::Cylinderf>(cylinder->radius,
+                                                     cylinder->length);
+
+      } else if (urdf_geometry->type == urdf::Geometry::MESH) {
+        const auto mesh = urdf::dynamic_pointer_cast<urdf::Mesh>(urdf_geometry);
+        coll_geom = load_fcl_geometry(urdf_path, mesh->filename);
+      }
+    }
+    coll_geoms.push_back(coll_geom);
+  }
+
   // construct joints and joint_ids, and numbering joint id
   std::vector<urdf::JointSharedPtr> joints;
   std::unordered_map<std::string, int> joint_ids;
@@ -72,6 +108,7 @@ KinematicModel::KinematicModel(const std::string &xml_string) {
   root_link_id_ = link_ids[root_link_->name];
   links_ = links;
   link_ids_ = link_ids;
+  coll_geoms_ = coll_geoms;
   joints_ = joints;
   joint_ids_ = joint_ids;
   num_dof_ = num_dof;

test/test_kinematics.cpp

@@ -105,10 +105,9 @@ TEST(KINEMATICS, AllTest) {
   size_t n_links = link_names.size();
 
   // test main
-  const std::string urdf_file = "../data/pr2.urdf";
-  const auto xml_string = load_urdf(urdf_file);
-  auto kin = KinematicModel(xml_string);
-  auto kin2 = KinematicModel(xml_string);
+  const std::string urdf_file_path = "../data/pr2.urdf";
+  auto kin = KinematicModel(urdf_file_path);
+  auto kin2 = KinematicModel(urdf_file_path);
 
   { // add new link to the robot
     std::vector<std::string> strvec = {"r_upper_arm_link"};