Merge pull request #77 from haosulab/one-link-multiple-geom

One link multiple geom

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), pose(Isometry3<S>::Identity()) {}
+
+  /**
+   * 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);
+
+  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 struct FCLObject<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::CollisionObjectPtr<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,10 +164,8 @@ class FCLModelTpl {
   std::string package_dir_;
   bool use_convex_ {};
 
-  std::vector<fcl::CollisionObjectPtr<S>> collision_objects_;
+  std::vector<FCLObjectPtr<S>> collision_objects_;
   std::vector<std::string> collision_link_names_;
-  std::vector<std::string> parent_link_names_;
-  std::vector<Isometry3<S>> collision_origin2link_poses_;
   std::vector<std::pair<size_t, size_t>> collision_pairs_;
 
   std::vector<std::string> user_link_names_;

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_ {};

include/mplib/planning_world.h

@@ -10,7 +10,6 @@
 #include "mplib/core/attached_body.h"
 #include "mplib/macros/class_forward.h"
 #include "mplib/types.h"
-#include "mplib/utils/color_printing.h"
 
 namespace mplib {
 
@@ -41,6 +40,8 @@ class PlanningWorldTpl {
   using CollisionObjectPtr = fcl::CollisionObjectPtr<S>;
   using AllowedCollisionMatrix = collision_detection::AllowedCollisionMatrix;
   using AllowedCollisionMatrixPtr = collision_detection::AllowedCollisionMatrixPtr;
+  using FCLObject = collision_detection::FCLObject<S>;
+  using FCLObjectPtr = collision_detection::FCLObjectPtr<S>;
   // using DynamicAABBTreeCollisionManager = fcl::DynamicAABBTreeCollisionManager<S>;
   using BroadPhaseCollisionManagerPtr = fcl::BroadPhaseCollisionManagerPtr<S>;
 
@@ -61,7 +62,7 @@ class PlanningWorldTpl {
    */
   PlanningWorldTpl(const std::vector<ArticulatedModelPtr> &articulations,
                    const std::vector<std::string> &articulation_names,
-                   const std::vector<CollisionObjectPtr> &normal_objects = {},
+                   const std::vector<FCLObjectPtr> &normal_objects = {},
                    const std::vector<std::string> &normal_object_names = {});
 
   /// @brief Gets names of all articulations in world (unordered)
@@ -134,12 +135,12 @@ class PlanningWorldTpl {
   std::vector<std::string> getNormalObjectNames() const;
 
   /**
-   * Gets the normal object (CollisionObjectPtr) with given name
+   * Gets the normal object (``FCLObjectPtr``) with given name
    *
    * @param name: name of the normal object
    * @return: the normal object with given name or ``nullptr`` if not found.
    */
-  CollisionObjectPtr getNormalObject(const std::string &name) const {
+  FCLObjectPtr getNormalObject(const std::string &name) const {
     auto it = normal_object_map_.find(name);
     return it != normal_object_map_.end() ? it->second : nullptr;
   }
@@ -155,16 +156,27 @@ class PlanningWorldTpl {
   }
 
   /**
-   * Adds a normal object (CollisionObjectPtr) with given name to world
+   * Adds a normal object containing multiple collision objects (``FCLObjectPtr``) with
+   * given name to world
    *
    * @param name: name of the collision object
    * @param collision_object: collision object to be added
    */
-  void addNormalObject(const std::string &name,
-                       const CollisionObjectPtr &collision_object) {
+  // TODO(merge): remove name
+  void addNormalObject(const std::string &name, const FCLObjectPtr &collision_object) {
     normal_object_map_[name] = collision_object;
   }
 
+  /**
+   * Adds a normal object (``CollisionObjectPtr``) with given name to world
+   *
+   * @param name: name of the collision object
+   * @param collision_object: collision object to be added
+   */
+  // TODO(merge): remove name
+  void addNormalObject(const std::string &name,
+                       const CollisionObjectPtr &collision_object);
+
   /**
    * Adds a point cloud as a collision object with given name to world
    *
@@ -445,8 +457,7 @@ class PlanningWorldTpl {
 
  private:
   std::unordered_map<std::string, ArticulatedModelPtr> articulation_map_;
-  // TODO: add name to CollisionObject similar as ArticulatedModel
-  std::unordered_map<std::string, CollisionObjectPtr> normal_object_map_;
+  std::unordered_map<std::string, FCLObjectPtr> normal_object_map_;
 
   // TODO: can planned_articulations_ be unordered_map? (setQposAll)
   std::map<std::string, ArticulatedModelPtr> planned_articulation_map_;