passes all tests

pybind/collision_detection/fcl/pybind_fcl.cpp

@@ -9,6 +9,7 @@
 #include <pybind11/stl.h>
 
 #include "docstring/collision_detection/fcl/fcl.h"
+#include "mplib/collision_detection/fcl/types.h"
 #include "mplib/types.h"
 #include "mplib/utils/conversion.h"
 #include "pybind_macros.hpp"
@@ -41,6 +42,7 @@ using DistanceResult = fcl::DistanceResult<S>;
 using Contact = fcl::Contact<S>;
 using ContactPoint = fcl::ContactPoint<S>;
 using CostSource = fcl::CostSource<S>;
+using FCLObject = fcl::FCLObject<S>;
 
 void build_pyfcl(py::module &m) {
   // Data type
@@ -328,6 +330,12 @@ void build_pyfcl(py::module &m) {
       .def_readonly("cost_density", &CostSource::cost_density)
       .def_readonly("total_cost", &CostSource::total_cost);
 
+  // FCLObject
+  auto PyFCLObject = py::class_<FCLObject, std::shared_ptr<FCLObject>>(m, "FCLObject");
+  PyFCLObject.def(py::init<>())
+      .def_readwrite("collision_objects", &FCLObject::collision_objects_)
+      .def_readwrite("tfs", &FCLObject::tfs_);
+
   // collide / distance functions
   m.def("collide",
         [](const CollisionObject *o1, const CollisionObject *o2,

src/collision_detection/fcl/fcl_model.cpp

@@ -65,7 +65,7 @@ void FCLModelTpl<S>::init(const urdf::ModelInterfaceSharedPtr &urdf_model,
 template <typename S>
 void FCLModelTpl<S>::dfsParseTree(const urdf::LinkConstSharedPtr &link,
                                   const std::string &parent_link_name) {
-  if (link->collision)
+  if (link->collision) {
     for (const auto &geom : link->collision_array) {
       auto geom_model = geom->geometry;
       fcl::CollisionGeometryPtr<S> collision_geometry;
@@ -121,6 +121,7 @@ void FCLModelTpl<S>::dfsParseTree(const urdf::LinkConstSharedPtr &link,
       collision_link_names_.push_back(link->name);
       parent_link_names_.push_back(parent_link_name);
     }
+  }
   for (const auto &child : link->child_links) dfsParseTree(child, link->name);
 }
 
@@ -182,8 +183,6 @@ void FCLModelTpl<S>::removeCollisionPairsFromSRDF(const std::string &srdf_filena
 template <typename S>
 void FCLModelTpl<S>::updateCollisionObjects(
     const std::vector<Isometry3<S>> &link_pose) const {
-  ASSERT(link_pose.size() == collision_objects_.size(),
-         "The size of link poses does not match the size of collision objects.");
   for (size_t i = 0; i < collision_objects_.size(); i++) {
     auto link_i = collision_link_user_indices_[i];
     auto link_wrt_world = link_pose[link_i];

src/collision_detection/fcl/fcl_utils.cpp

@@ -68,11 +68,13 @@ void collideFCLObjects(const fcl::FCLObject<S> &o1, const fcl::FCLObject<S> &o2,
     for (const auto &co_obj2 : o2.collision_objects_) {
       fcl::CollisionResult<S> partial_result;
       auto cost_sources = std::vector<fcl::CostSource<S>>();
-      partial_result.getCostSources(cost_sources);
       fcl::collide(co_obj1.get(), co_obj2.get(), request, partial_result);
       for (size_t i = 0; i < partial_result.numContacts(); i++) {
         result.addContact(partial_result.getContact(i));
-        result.addCostSource(cost_sources[i], request.num_max_cost_sources);
+      }
+      partial_result.getCostSources(cost_sources);
+      for (auto &cost_source : cost_sources) {
+        result.addCostSource(cost_source, request.num_max_cost_sources);
       }
     }
   }

tests/test_fcl_model.py

@@ -42,16 +42,26 @@ def setUp(self):
         for i, link_name in enumerate(self.collision_link_names):
             link_idx = self.pinocchio_model.get_link_names().index(link_name)
             link_pose = self.pinocchio_model.get_link_pose(link_idx)
-            self.model.get_collision_objects()[i].set_transformation(link_pose)
+            self.model.get_collision_objects()[i].collision_objects[
+                0
+            ].set_transformation(link_pose)
 
     def test_get_collision_objects(self):
         self.assertEqual(
             self.model.get_collision_link_names(), self.collision_link_names
         )
         for i, link_name in enumerate(self.collision_link_names):
             pinocchio_idx = self.pinocchio_model.get_link_names().index(link_name)
-            pos = self.model.get_collision_objects()[i].get_translation()
-            quat = mat2quat(self.model.get_collision_objects()[i].get_rotation())
+            pos = (
+                self.model.get_collision_objects()[i]
+                .collision_objects[0]
+                .get_translation()
+            )
+            quat = mat2quat(
+                self.model.get_collision_objects()[i]
+                .collision_objects[0]
+                .get_rotation()
+            )
             pinocchio_pose = self.pinocchio_model.get_link_pose(pinocchio_idx)
             self.assertTrue(np.allclose(pos, pinocchio_pose[:3]))
             self.assertAlmostEqual(abs(quat.dot(pinocchio_pose[3:])), 1)