Change all CollisionObject to FCLObject; Fix merge

include/mplib/collision_detection/fcl/collision_common.h

@@ -0,0 +1,97 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <fcl/narrowphase/collision.h>
+#include <fcl/narrowphase/distance.h>
+
+#include "mplib/collision_detection/fcl/types.h"
+#include "mplib/macros/class_forward.h"
+#include "mplib/types.h"
+
+namespace mplib::collision_detection::fcl {
+
+// FCLObjectPtr
+MPLIB_STRUCT_TEMPLATE_FORWARD(FCLObject);
+
+/**
+ * A general high-level object which consists of multiple FCLCollisionObjects.
+ * It is the top level data structure which is used in the collision checking process.
+ *
+ * Mimicking MoveIt2's ``collision_detection::FCLObject`` and
+ * ``collision_detection::World::Object``
+ *
+ * https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1FCLObject.html
+ * https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1World_1_1Object.html
+ */
+template <typename S>
+struct FCLObject {
+  /**
+   * Construct a new FCLObject with the given name
+   *
+   * @param name: name of this FCLObject
+   */
+  FCLObject(const std::string &name) : name(name) {}
+
+  /**
+   * Construct a new FCLObject with the given name and shapes
+   *
+   * @param name: name of this FCLObject
+   * @param pose: pose of this FCLObject. All shapes are relative to this pose
+   * @param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr``
+   * @param shape_poses: relative poses from this FCLObject to each collision shape
+   */
+  FCLObject(const std::string &name, const Isometry3<S> &pose,
+            const std::vector<fcl::CollisionObjectPtr<S>> &shapes,
+            const std::vector<Isometry3<S>> &shape_poses)
+      : name(name), pose(pose), shapes(shapes), shape_poses(shape_poses) {}
+
+  std::string name;   ///< Name of this FCLObject
+  Isometry3<S> pose;  ///< Pose of this FCLObject. All shapes are relative to this pose
+  /// All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject
+  std::vector<fcl::CollisionObjectPtr<S>> shapes;
+  /// Relative poses from this FCLObject to each collision shape
+  std::vector<Isometry3<S>> shape_poses;
+};
+
+/**
+ * Collision function between two ``FCLObject``
+ *
+ * @param[in] obj1: the first object
+ * @param[in] obj2: the second object
+ * @param[in] request: ``fcl::CollisionRequest``
+ * @param[out] result: ``fcl::CollisionResult``
+ * @return: number of contacts generated between the two objects
+ */
+template <typename S>
+size_t collide(const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,
+               const fcl::CollisionRequest<S> &request,
+               fcl::CollisionResult<S> &result);
+
+/**
+ * Distance function between two ``FCLObject``
+ *
+ * @param[in] obj1: the first object
+ * @param[in] obj2: the second object
+ * @param[in] request: ``fcl::DistanceRequest``
+ * @param[out] result: ``fcl::DistanceResult``
+ * @return: minimum distance generated between the two objects
+ */
+template <typename S>
+S distance(const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,
+           const fcl::DistanceRequest<S> &request, fcl::DistanceResult<S> &result);
+
+// Explicit Template Instantiation Declaration =========================================
+#define DECLARE_TEMPLATE_FCL_COMMON(S)                                           \
+  extern template size_t collide<S>(                                             \
+      const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,                  \
+      const fcl::CollisionRequest<S> &request, fcl::CollisionResult<S> &result); \
+  extern template S distance<S>(                                                 \
+      const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,                  \
+      const fcl::DistanceRequest<S> &request, fcl::DistanceResult<S> &result)
+
+DECLARE_TEMPLATE_FCL_COMMON(float);
+DECLARE_TEMPLATE_FCL_COMMON(double);
+
+}  // namespace mplib::collision_detection::fcl

include/mplib/collision_detection/fcl/fcl_model.h

@@ -10,7 +10,7 @@
 #include <urdf_model/types.h>
 #include <urdf_world/types.h>
 
-#include "mplib/collision_detection/fcl/types.h"
+#include "mplib/collision_detection/fcl/collision_common.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
 
@@ -54,24 +54,23 @@ class FCLModelTpl {
    * Constructs a FCLModel from URDF string and collision links
    *
    * @param urdf_string: URDF string (without visual/collision elements for links)
-   * @param collision_links: Collision link names and the vector of CollisionObjectPtr.
-   *    Format is: ``[(link_name, [CollisionObjectPtr, ...]), ...]``.
+   * @param collision_links: Vector of collision link names and FCLObjectPtr.
+   *    Format is: ``[(link_name, FCLObjectPtr), ...]``.
    *    The collision objects are at the shape's local_pose.
    * @param verbose: print debug information. Default: ``false``.
    * @return: a unique_ptr to FCLModel
    */
   static std::unique_ptr<FCLModelTpl<S>> createFromURDFString(
       const std::string &urdf_string,
-      const std::vector<std::pair<std::string, std::vector<fcl::CollisionObjectPtr<S>>>>
-          &collision_links,
+      const std::vector<std::pair<std::string, FCLObjectPtr<S>>> &collision_links,
       bool verbose = false);
 
   /**
    * Get the collision objects of the FCL model.
    *
    * @return: all collision objects of the FCL model
    */
-  const std::vector<fcl::FCLObject<S>> &getCollisionObjects() const {
+  const std::vector<FCLObjectPtr<S>> &getCollisionObjects() const {
     return collision_objects_;
   }
 
@@ -126,15 +125,20 @@ class FCLModelTpl {
    *
    * @param link_poses: list of link poses in the order of the link order
    */
-  void updateCollisionObjects(const std::vector<Vector7<S>> &link_pose) const;
+  void updateCollisionObjects(const std::vector<Vector7<S>> &link_poses) const;
 
-  void updateCollisionObjects(const std::vector<Isometry3<S>> &link_pose) const;
+  /**
+   * Update the collision objects of the FCL model.
+   *
+   * @param link_poses: list of link poses in the order of the link order
+   */
+  void updateCollisionObjects(const std::vector<Isometry3<S>> &link_poses) const;
 
   /**
    * Perform self-collision checking.
    *
    * @param request: collision request
-   * @return: ``true`` if any collision pair collides
+   * @return: ``true`` if any collision pair collides and ``false`` otherwise.
    */
   bool collide(
       const fcl::CollisionRequest<S> &request = fcl::CollisionRequest<S>()) const;
@@ -160,7 +164,7 @@ class FCLModelTpl {
   std::string package_dir_;
   bool use_convex_ {};
 
-  std::vector<fcl::FCLObject<S>> collision_objects_;
+  std::vector<FCLObjectPtr<S>> collision_objects_;
   std::vector<std::string> collision_link_names_;
   std::vector<std::pair<size_t, size_t>> collision_pairs_;
 

include/mplib/collision_detection/fcl/fcl_utils.h

@@ -2,8 +2,6 @@
 
 #include <string>
 
-#include <fcl/narrowphase/collision.h>
-
 #include "mplib/collision_detection/fcl/types.h"
 #include "mplib/types.h"
 
@@ -32,21 +30,12 @@ template <typename S>
 fcl::ConvexPtr<S> loadMeshAsConvex(const std::string &mesh_path,
                                    const Vector3<S> &scale);
 
-template <typename S>
-void collideFCLObjects(const fcl::FCLObject<S> &o1, const fcl::FCLObject<S> &o2,
-                       const fcl::CollisionRequest<S> &request,
-                       fcl::CollisionResult<S> &result);
-
 // Explicit Template Instantiation Declaration =========================================
 #define DECLARE_TEMPLATE_FCL_UTILS(S)                                                 \
   extern template fcl::BVHModel_OBBRSSPtr<S> loadMeshAsBVH<S>(                        \
       const std::string &mesh_path, const Vector3<S> &scale);                         \
   extern template fcl::ConvexPtr<S> loadMeshAsConvex<S>(const std::string &mesh_path, \
-                                                        const Vector3<S> &scale);     \
-  extern template void collideFCLObjects(const fcl::FCLObject<S> &o1,                 \
-                                         const fcl::FCLObject<S> &o2,                 \
-                                         const fcl::CollisionRequest<S> &request,     \
-                                         fcl::CollisionResult<S> &result)
+                                                        const Vector3<S> &scale)
 
 DECLARE_TEMPLATE_FCL_UTILS(float);
 DECLARE_TEMPLATE_FCL_UTILS(double);

include/mplib/collision_detection/fcl/types.h

@@ -8,8 +8,6 @@
 #include <fcl/geometry/shape/convex.h>
 #include <fcl/narrowphase/collision_object.h>
 
-#include "mplib/types.h"
-
 namespace mplib::collision_detection::fcl {
 
 // Namespace alias
@@ -34,17 +32,8 @@ using BVHModel_OBBRSS = fcl::BVHModel<fcl::OBBRSS<S>>;
 template <typename S>
 using BVHModel_OBBRSSPtr = std::shared_ptr<BVHModel_OBBRSS<S>>;
 
-template <typename S>
-using CollisionObjectPtr = std::shared_ptr<fcl::CollisionObject<S>>;
-
 template <typename S>
 using BroadPhaseCollisionManagerPtr =
     std::shared_ptr<fcl::BroadPhaseCollisionManager<S>>;
 
-template <typename S>
-struct FCLObject {
-   std::vector<CollisionObjectPtr<S>> collision_objects_;
-   std::vector<mplib::Isometry3<S>> tfs_;
-};
-
 }  // namespace fcl

include/mplib/collision_detection/types.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "mplib/collision_detection/collision_common.h"
+#include "mplib/collision_detection/fcl/collision_common.h"
 #include "mplib/collision_detection/fcl/fcl_model.h"
 
 namespace mplib {
@@ -14,6 +15,12 @@ using FCLModelTpl = fcl::FCLModelTpl<S>;
 template <typename S>
 using FCLModelTplPtr = fcl::FCLModelTplPtr<S>;
 
+template <typename S>
+using FCLObject = fcl::FCLObject<S>;
+
+template <typename S>
+using FCLObjectPtr = fcl::FCLObjectPtr<S>;
+
 }  // namespace collision_detection
 
 // Export classes from inner namespace to mplib namespace

include/mplib/core/articulated_model.h

@@ -5,7 +5,6 @@
 #include <utility>
 #include <vector>
 
-#include "mplib/collision_detection/fcl/types.h"
 #include "mplib/collision_detection/types.h"
 #include "mplib/kinematics/types.h"
 #include "mplib/macros/class_forward.h"
@@ -58,8 +57,8 @@ class ArticulatedModelTpl {
    *
    * @param urdf_string: URDF string (without visual/collision elements for links)
    * @param srdf_string: SRDF string (only disable_collisions element)
-   * @param collision_links: Collision link names and the vector of CollisionObjectPtr.
-   *    Format is: ``[(link_name, [CollisionObjectPtr, ...]), ...]``.
+   * @param collision_links: Vector of collision link names and FCLObjectPtr.
+   *    Format is: ``[(link_name, FCLObjectPtr), ...]``.
    *    The collision objects are at the shape's local_pose.
    * @param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis
    * @param link_names: list of links that are considered for planning
@@ -69,7 +68,7 @@ class ArticulatedModelTpl {
    */
   static std::unique_ptr<ArticulatedModelTpl<S>> createFromURDFString(
       const std::string &urdf_string, const std::string &srdf_string,
-      const std::vector<std::pair<std::string, std::vector<fcl::CollisionObjectPtr<S>>>>
+      const std::vector<std::pair<std::string, collision_detection::FCLObjectPtr<S>>>
           &collision_links,
       const Vector3<S> &gravity = Vector3<S> {0, 0, -9.81},
       const std::vector<std::string> &link_names = {},

include/mplib/core/attached_body.h

@@ -3,6 +3,7 @@
 #include <string>
 #include <vector>
 
+#include "mplib/collision_detection/types.h"
 #include "mplib/core/articulated_model.h"
 #include "mplib/kinematics/types.h"
 #include "mplib/macros/class_forward.h"
@@ -26,10 +27,9 @@ template <typename S>
 class AttachedBodyTpl {
  public:
   // Common type alias
-  using CollisionObjectPtr = fcl::CollisionObjectPtr<S>;
+  using FCLObjectPtr = collision_detection::FCLObjectPtr<S>;
   using ArticulatedModelPtr = ArticulatedModelTplPtr<S>;
 
-  // TODO(merge): Support multi collision geometry
   /**
    * Construct an attached body for a specified link.
    *
@@ -40,16 +40,16 @@ class AttachedBodyTpl {
    * @param pose: attached pose (relative pose from attached link to object)
    * @param touch_links: the link names that the attached body touches
    */
-  AttachedBodyTpl(const std::string &name, const CollisionObjectPtr &object,
+  AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object,
                   const ArticulatedModelPtr &attached_articulation,
                   int attached_link_id, const Isometry3<S> &pose,
                   const std::vector<std::string> &touch_links = {});
 
   /// @brief Gets the attached object name
   const std::string &getName() const { return name_; }
 
-  /// @brief Gets the attached object (CollisionObjectPtr)
-  CollisionObjectPtr getObject() const { return object_; }
+  /// @brief Gets the attached object (``FCLObjectPtr``)
+  FCLObjectPtr getObject() const { return object_; }
 
   /// @brief Gets the articulation this body is attached to
   ArticulatedModelPtr getAttachedArticulation() const { return attached_articulation_; }
@@ -69,7 +69,7 @@ class AttachedBodyTpl {
   }
 
   /// @brief Updates the global pose of the attached object using current state
-  void updatePose() const { object_->setTransform(getGlobalPose()); }
+  void updatePose() const;
 
   /// @brief Gets the link names that the attached body touches
   const std::vector<std::string> &getTouchLinks() const { return touch_links_; }
@@ -81,7 +81,7 @@ class AttachedBodyTpl {
 
  private:
   std::string name_;
-  CollisionObjectPtr object_;
+  FCLObjectPtr object_;
   ArticulatedModelPtr attached_articulation_;
   kinematics::PinocchioModelTplPtr<S> pinocchio_model_;
   int attached_link_id_ {};