diff --git a/include/mplib/collision_detection/fcl/collision_common.h b/include/mplib/collision_detection/fcl/collision_common.h new file mode 100644 index 00000000..ea2cca0b --- /dev/null +++ b/include/mplib/collision_detection/fcl/collision_common.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include + +#include +#include + +#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 +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::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 &pose, + const std::vector> &shapes, + const std::vector> &shape_poses); + + std::string name; ///< Name of this FCLObject + Isometry3 pose; ///< Pose of this FCLObject. All shapes are relative to this pose + /// All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject + std::vector> shapes; + /// Relative poses from this FCLObject to each collision shape + std::vector> 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 +size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const fcl::CollisionRequest &request, + fcl::CollisionResult &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 +S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const fcl::DistanceRequest &request, fcl::DistanceResult &result); + +// Explicit Template Instantiation Declaration ========================================= +#define DECLARE_TEMPLATE_FCL_COMMON(S) \ + extern template struct FCLObject; \ + extern template size_t collide( \ + const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ + const fcl::CollisionRequest &request, fcl::CollisionResult &result); \ + extern template S distance( \ + const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ + const fcl::DistanceRequest &request, fcl::DistanceResult &result) + +DECLARE_TEMPLATE_FCL_COMMON(float); +DECLARE_TEMPLATE_FCL_COMMON(double); + +} // namespace mplib::collision_detection::fcl diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 60f791de..0fd88133 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -10,7 +10,7 @@ #include #include -#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,16 +54,15 @@ 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> createFromURDFString( const std::string &urdf_string, - const std::vector>>> - &collision_links, + const std::vector>> &collision_links, bool verbose = false); /** @@ -71,7 +70,7 @@ class FCLModelTpl { * * @return: all collision objects of the FCL model */ - const std::vector> &getCollisionObjects() const { + const std::vector> &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> &link_pose) const; + void updateCollisionObjects(const std::vector> &link_poses) const; - void updateCollisionObjects(const std::vector> &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> &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 &request = fcl::CollisionRequest()) const; @@ -160,10 +164,8 @@ class FCLModelTpl { std::string package_dir_; bool use_convex_ {}; - std::vector> collision_objects_; + std::vector> collision_objects_; std::vector collision_link_names_; - std::vector parent_link_names_; - std::vector> collision_origin2link_poses_; std::vector> collision_pairs_; std::vector user_link_names_; diff --git a/include/mplib/collision_detection/types.h b/include/mplib/collision_detection/types.h index 13960752..5f8a6738 100644 --- a/include/mplib/collision_detection/types.h +++ b/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; template using FCLModelTplPtr = fcl::FCLModelTplPtr; +template +using FCLObject = fcl::FCLObject; + +template +using FCLObjectPtr = fcl::FCLObjectPtr; + } // namespace collision_detection // Export classes from inner namespace to mplib namespace diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index c3b8a6ec..0110502b 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -5,7 +5,6 @@ #include #include -#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> createFromURDFString( const std::string &urdf_string, const std::string &srdf_string, - const std::vector>>> + const std::vector>> &collision_links, const Vector3 &gravity = Vector3 {0, 0, -9.81}, const std::vector &link_names = {}, diff --git a/include/mplib/core/attached_body.h b/include/mplib/core/attached_body.h index 3dab70a6..651329a7 100644 --- a/include/mplib/core/attached_body.h +++ b/include/mplib/core/attached_body.h @@ -3,6 +3,7 @@ #include #include +#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 class AttachedBodyTpl { public: // Common type alias - using CollisionObjectPtr = fcl::CollisionObjectPtr; + using FCLObjectPtr = collision_detection::FCLObjectPtr; using ArticulatedModelPtr = ArticulatedModelTplPtr; - // TODO(merge): Support multi collision geometry /** * Construct an attached body for a specified link. * @@ -40,7 +40,7 @@ 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 &pose, const std::vector &touch_links = {}); @@ -48,8 +48,8 @@ class AttachedBodyTpl { /// @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 &getTouchLinks() const { return touch_links_; } @@ -81,7 +81,7 @@ class AttachedBodyTpl { private: std::string name_; - CollisionObjectPtr object_; + FCLObjectPtr object_; ArticulatedModelPtr attached_articulation_; kinematics::PinocchioModelTplPtr pinocchio_model_; int attached_link_id_ {}; diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 0430e42a..6aa412a0 100644 --- a/include/mplib/planning_world.h +++ b/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; using AllowedCollisionMatrix = collision_detection::AllowedCollisionMatrix; using AllowedCollisionMatrixPtr = collision_detection::AllowedCollisionMatrixPtr; + using FCLObject = collision_detection::FCLObject; + using FCLObjectPtr = collision_detection::FCLObjectPtr; // using DynamicAABBTreeCollisionManager = fcl::DynamicAABBTreeCollisionManager; using BroadPhaseCollisionManagerPtr = fcl::BroadPhaseCollisionManagerPtr; @@ -61,7 +62,7 @@ class PlanningWorldTpl { */ PlanningWorldTpl(const std::vector &articulations, const std::vector &articulation_names, - const std::vector &normal_objects = {}, + const std::vector &normal_objects = {}, const std::vector &normal_object_names = {}); /// @brief Gets names of all articulations in world (unordered) @@ -134,12 +135,12 @@ class PlanningWorldTpl { std::vector 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 articulation_map_; - // TODO: add name to CollisionObject similar as ArticulatedModel - std::unordered_map normal_object_map_; + std::unordered_map normal_object_map_; // TODO: can planned_articulations_ be unordered_map? (setQposAll) std::map planned_articulation_map_; diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 39ac06c3..4de4d056 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -27,9 +27,7 @@ class ArticulatedModel: def create_from_urdf_string( urdf_string: str, srdf_string: str, - collision_links: list[ - tuple[str, list[collision_detection.fcl.CollisionObject]] - ], + collision_links: list[tuple[str, collision_detection.fcl.FCLObject]], *, gravity: numpy.ndarray[ tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] @@ -43,9 +41,9 @@ class ArticulatedModel: :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, ...]), - ...]``. The collision objects are at the shape's local_pose. + :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 :param joint_names: list of joints that are considered for planning @@ -215,7 +213,7 @@ class AttachedBody: def __init__( self, name: str, - object: collision_detection.fcl.CollisionObject, + object: collision_detection.fcl.FCLObject, attached_articulation: ArticulatedModel, attached_link_id: int, pose: numpy.ndarray[ @@ -249,9 +247,9 @@ class AttachedBody: """ Gets the attached object name """ - def get_object(self) -> collision_detection.fcl.CollisionObject: + def get_object(self) -> collision_detection.fcl.FCLObject: """ - Gets the attached object (CollisionObjectPtr) + Gets the attached object (``FCLObjectPtr``) """ def get_pose(self) -> ...: """ @@ -294,7 +292,7 @@ class PlanningWorld: self, articulations: list[ArticulatedModel], articulation_names: list[str], - normal_objects: list[collision_detection.fcl.CollisionObject] = [], + normal_objects: list[collision_detection.fcl.FCLObject] = [], normal_object_names: list[str] = [], ) -> None: """ @@ -315,11 +313,23 @@ class PlanningWorld: :param model: articulated model to be added :param planned: whether the articulation is being planned """ + @typing.overload + def add_normal_object( + self, name: str, collision_object: collision_detection.fcl.FCLObject + ) -> None: + """ + 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 + """ + @typing.overload def add_normal_object( self, name: str, collision_object: collision_detection.fcl.CollisionObject ) -> None: """ - Adds a normal object (CollisionObjectPtr) with given name to world + 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 @@ -609,9 +619,9 @@ class PlanningWorld: :param name: name of the attached body :return: the attached body with given name or ``None`` if not found. """ - def get_normal_object(self, name: str) -> collision_detection.fcl.CollisionObject: + def get_normal_object(self, name: str) -> collision_detection.fcl.FCLObject: """ - 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 ``None`` if not found. diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 40401f2b..2743e97f 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -24,6 +24,7 @@ __all__ = [ "DistanceResult", "Ellipsoid", "FCLModel", + "FCLObject", "GJKSolverType", "GST_INDEP", "GST_LIBCCD", @@ -558,7 +559,7 @@ class FCLModel: @staticmethod def create_from_urdf_string( urdf_string: str, - collision_links: list[tuple[str, list[CollisionObject]]], + collision_links: list[tuple[str, ...]], *, verbose: bool = False, ) -> FCLModel: @@ -566,9 +567,9 @@ class FCLModel: 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, ...]), - ...]``. The collision objects are at the shape's local_pose. + :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 """ @@ -589,7 +590,7 @@ class FCLModel: 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. """ def collide_full(self, request: CollisionRequest = ...) -> list[CollisionResult]: """ @@ -599,7 +600,7 @@ class FCLModel: :return: list of CollisionResult for each collision pair """ def get_collision_link_names(self) -> list[str]: ... - def get_collision_objects(self) -> list[CollisionObject]: + def get_collision_objects(self) -> list[...]: """ Get the collision objects of the FCL model. @@ -640,6 +641,56 @@ class FCLModel: :param link_poses: list of link poses in the order of the link order """ +class 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 + """ + @staticmethod + @typing.overload + def __init__(*args, **kwargs) -> None: + """ + 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 + """ + @typing.overload + def __init__(self, name: str) -> None: + """ + Construct a new FCLObject with the given name + + :param name: name of this FCLObject + """ + @property + def name(self) -> str: + """ + Name of this FCLObject + """ + @property + def pose(self) -> ...: + """ + Pose of this FCLObject. All shapes are relative to this pose + """ + @property + def shape_poses(self) -> list[..., 3, 1, ...]: + """ + Relative poses from this FCLObject to each collision shape + """ + @property + def shapes(self) -> list[CollisionObject]: + """ + All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject + """ + class GJKSolverType: """ Members: @@ -879,11 +930,21 @@ class TriangleP(CollisionGeometry): :param c: vector of point ``z`` coordinates """ +@typing.overload def collide( - arg0: CollisionObject, arg1: CollisionObject, arg2: CollisionRequest + obj1: CollisionObject, obj2: CollisionObject, request: CollisionRequest = ... ) -> CollisionResult: ... +@typing.overload +def collide( + obj1: FCLObject, obj2: FCLObject, request: CollisionRequest = ... +) -> CollisionResult: ... +@typing.overload +def distance( + obj1: CollisionObject, obj2: CollisionObject, request: DistanceRequest = ... +) -> DistanceResult: ... +@typing.overload def distance( - arg0: CollisionObject, arg1: CollisionObject, arg2: DistanceRequest + obj1: FCLObject, obj2: FCLObject, request: DistanceRequest = ... ) -> DistanceResult: ... def load_mesh_as_BVH( mesh_path: str, diff --git a/pybind/collision_detection/fcl/pybind_collision_common.cpp b/pybind/collision_detection/fcl/pybind_collision_common.cpp new file mode 100644 index 00000000..11b69f9d --- /dev/null +++ b/pybind/collision_detection/fcl/pybind_collision_common.cpp @@ -0,0 +1,67 @@ +#include +#include +#include + +#include +#include +#include +#include + +#include "docstring/collision_detection/fcl/collision_common.h" +#include "mplib/collision_detection/fcl/collision_common.h" +#include "mplib/types.h" +#include "pybind_macros.hpp" + +namespace py = pybind11; + +namespace mplib::collision_detection::fcl { + +using CollisionRequest = fcl::CollisionRequest; +using CollisionResult = fcl::CollisionResult; +using DistanceRequest = fcl::DistanceRequest; +using DistanceResult = fcl::DistanceResult; + +void build_pyfcl_collision_common(py::module &m) { + auto PyFCLObject = py::class_, std::shared_ptr>>( + m, "FCLObject", DOC(mplib, collision_detection, fcl, FCLObject)); + + // TODO(merge): mplib.Pose + PyFCLObject + .def(py::init(), py::arg("name"), + DOC(mplib, collision_detection, fcl, FCLObject, FCLObject)) + .def(py::init &, + const std::vector> &, + const std::vector> &>(), + py::arg("name"), py::arg("pose"), py::arg("shapes"), py::arg("shape_poses"), + DOC(mplib, collision_detection, fcl, FCLObject, FCLObject, 2)) + .def_readonly("name", &FCLObject::name, + DOC(mplib, collision_detection, fcl, FCLObject, name)) + .def_readonly("pose", &FCLObject::pose, + DOC(mplib, collision_detection, fcl, FCLObject, pose)) + .def_readonly("shapes", &FCLObject::shapes, + DOC(mplib, collision_detection, fcl, FCLObject, shapes)) + .def_readonly("shape_poses", &FCLObject::shape_poses, + DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)); + + // collide / distance functions + m.def( + "collide", + [](const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const CollisionRequest &request) { + CollisionResult result; + collide(obj1, obj2, request, result); + return result; + }, + py::arg("obj1"), py::arg("obj2"), py::arg("request") = CollisionRequest()) + .def( + "distance", + [](const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const DistanceRequest &request) { + DistanceResult result; + distance(obj1, obj2, request, result); + return result; + }, + py::arg("obj1"), py::arg("obj2"), py::arg("request") = DistanceRequest()); +} + +} // namespace mplib::collision_detection::fcl diff --git a/pybind/collision_detection/fcl/pybind_fcl.cpp b/pybind/collision_detection/fcl/pybind_fcl.cpp index 7e69794d..d3221936 100644 --- a/pybind/collision_detection/fcl/pybind_fcl.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl.cpp @@ -368,19 +368,24 @@ void build_pyfcl(py::module &m) { .def_readonly("total_cost", &CostSource::total_cost); // collide / distance functions - m.def("collide", - [](const CollisionObject *o1, const CollisionObject *o2, - const CollisionRequest &request) { - CollisionResult result; - fcl::collide(o1, o2, request, result); - return result; - }) - .def("distance", [](const CollisionObject *o1, const CollisionObject *o2, - const DistanceRequest &request) { - DistanceResult result; - fcl::distance(o1, o2, request, result); - return result; - }); + m.def( + "collide", + [](const CollisionObject *obj1, const CollisionObject *obj2, + const CollisionRequest &request) { + CollisionResult result; + fcl::collide(obj1, obj2, request, result); + return result; + }, + py::arg("obj1"), py::arg("obj2"), py::arg("request") = CollisionRequest()) + .def( + "distance", + [](const CollisionObject *obj1, const CollisionObject *obj2, + const DistanceRequest &request) { + DistanceResult result; + fcl::distance(obj1, obj2, request, result); + return result; + }, + py::arg("obj1"), py::arg("obj2"), py::arg("request") = DistanceRequest()); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 11e84883..8f0b962f 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -20,7 +20,6 @@ namespace mplib::collision_detection::fcl { using FCLModel = FCLModelTpl; using CollisionRequest = fcl::CollisionRequest; -using CollisionObjectPtr = fcl::CollisionObjectPtr; void build_pyfcl_model(py::module &m) { auto PyFCLModel = py::class_>( @@ -33,7 +32,7 @@ void build_pyfcl_model(py::module &m) { .def_static( "create_from_urdf_string", [](const std::string &urdf_string, - const std::vector>> + const std::vector>> &collision_links, bool verbose) { std::shared_ptr fcl_model = diff --git a/pybind/collision_detection/pybind_collision_detection.cpp b/pybind/collision_detection/pybind_collision_detection.cpp index 17d2e5a6..5a4253d8 100644 --- a/pybind/collision_detection/pybind_collision_detection.cpp +++ b/pybind/collision_detection/pybind_collision_detection.cpp @@ -9,6 +9,7 @@ namespace fcl { void build_pyfcl(py::module &pyfcl); void build_pyfcl_model(py::module &pyfcl); void build_pyfcl_utils(py::module &pyfcl); +void build_pyfcl_collision_common(py::module &pyfcl); } // namespace fcl @@ -24,6 +25,7 @@ void build_pycollision_detection(py::module &pymp) { fcl::build_pyfcl(pyfcl); fcl::build_pyfcl_model(pyfcl); fcl::build_pyfcl_utils(pyfcl); + fcl::build_pyfcl_collision_common(pyfcl); } } // namespace mplib::collision_detection diff --git a/pybind/core/pybind_articulated_model.cpp b/pybind/core/pybind_articulated_model.cpp index a95d4ed1..9c5dae29 100644 --- a/pybind/core/pybind_articulated_model.cpp +++ b/pybind/core/pybind_articulated_model.cpp @@ -17,7 +17,7 @@ namespace py = pybind11; namespace mplib { using ArticulatedModel = ArticulatedModelTpl; -using CollisionObjectPtr = fcl::CollisionObjectPtr; +using FCLObjectPtr = collision_detection::FCLObjectPtr; void build_pyarticulated_model(py::module &pymp) { auto PyArticulatedModel = @@ -38,8 +38,7 @@ void build_pyarticulated_model(py::module &pymp) { .def_static( "create_from_urdf_string", [](const std::string &urdf_string, const std::string &srdf_string, - const std::vector>> - &collision_links, + const std::vector> &collision_links, const Vector3 &gravity, const std::vector &link_names, const std::vector &joint_names, bool verbose) { std::shared_ptr articulation = diff --git a/pybind/core/pybind_attached_body.cpp b/pybind/core/pybind_attached_body.cpp index d7b8f572..d8962c08 100644 --- a/pybind/core/pybind_attached_body.cpp +++ b/pybind/core/pybind_attached_body.cpp @@ -16,14 +16,14 @@ namespace py = pybind11; namespace mplib { using AttachedBody = AttachedBodyTpl; -using CollisionObjectPtr = fcl::CollisionObjectPtr; +using FCLObjectPtr = collision_detection::FCLObjectPtr; using ArticulatedModelPtr = ArticulatedModelTplPtr; void build_pyattached_body(py::module &pymp) { auto PyAttachedBody = py::class_>( pymp, "AttachedBody", DOC(mplib, AttachedBodyTpl)); PyAttachedBody - .def(py::init([](const std::string &name, const CollisionObjectPtr &object, + .def(py::init([](const std::string &name, const FCLObjectPtr &object, const ArticulatedModelPtr &attached_articulation, int attached_link_id, const Vector7 &posevec, const std::vector &touch_links) { diff --git a/pybind/docstring/collision_detection/fcl/collision_common.h b/pybind/docstring/collision_detection/fcl/collision_common.h new file mode 100644 index 00000000..2e67fdb6 --- /dev/null +++ b/pybind/docstring/collision_detection/fcl/collision_common.h @@ -0,0 +1,86 @@ +#pragma once + +/* + This file contains docstrings for use in the Python bindings. + Do not edit! They were automatically extracted by mkdoc.py. + */ + +#define __EXPAND(x) x +#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT +#define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) +#define __CAT1(a, b) a ## b +#define __CAT2(a, b) __CAT1(a, b) +#define __DOC1(n1) __doc_##n1 +#define __DOC2(n1, n2) __doc_##n1##_##n2 +#define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 +#define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 +#define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 +#define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 +#define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 +#define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) + +#if defined(__GNUG__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif + +static const char *__doc_mplib_collision_detection_fcl_FCLObject = +R"doc(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)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_FCLObject = +R"doc( +Construct a new FCLObject with the given name + +:param name: name of this FCLObject)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_FCLObject_2 = +R"doc( +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)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_name = R"doc(Name of this FCLObject)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_pose = R"doc(Pose of this FCLObject. All shapes are relative to this pose)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_shape_poses = R"doc(Relative poses from this FCLObject to each collision shape)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLObject_shapes = R"doc(All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject)doc"; + +static const char *__doc_mplib_collision_detection_fcl_collide = +R"doc( +Collision function between two ``FCLObject`` + +:param obj1: the first object +:param obj2: the second object +:param request: ``fcl::CollisionRequest`` +:param result: ``fcl::CollisionResult`` +:return: number of contacts generated between the two objects)doc"; + +static const char *__doc_mplib_collision_detection_fcl_distance = +R"doc( +Distance function between two ``FCLObject`` + +:param obj1: the first object +:param obj2: the second object +:param request: ``fcl::DistanceRequest`` +:param result: ``fcl::DistanceResult`` +:return: minimum distance generated between the two objects)doc"; + +/* ----- Begin of custom docstring section ----- */ + +/* ----- End of custom docstring section ----- */ + +#if defined(__GNUG__) +#pragma GCC diagnostic pop +#endif diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index a600e2c8..78b30c91 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -54,7 +54,7 @@ R"doc( Perform self-collision checking. :param request: collision request -:return: ``True`` if any collision pair collides)doc"; +:return: ``True`` if any collision pair collides and ``False`` otherwise.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_collideFull = R"doc( @@ -68,9 +68,9 @@ R"doc( 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, ...]), - ...]``. The collision objects are at the shape's local_pose. +: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)doc"; @@ -131,7 +131,9 @@ Update the collision objects of the FCL model. static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_updateCollisionObjects_2 = R"doc( -)doc"; +Update the collision objects of the FCL model. + +:param link_poses: list of link poses in the order of the link order)doc"; /* ----- Begin of custom docstring section ----- */ diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index 5d680704..bf5099a4 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -53,9 +53,9 @@ Constructs an ArticulatedModel from URDF/SRDF strings and collision links :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, ...]), - ...]``. The collision objects are at the shape's local_pose. +: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 :param joint_names: list of joints that are considered for planning diff --git a/pybind/docstring/core/attached_body.h b/pybind/docstring/core/attached_body.h index 6bcb4973..dc0cc15f 100644 --- a/pybind/docstring/core/attached_body.h +++ b/pybind/docstring/core/attached_body.h @@ -61,7 +61,7 @@ Gets the attached object name)doc"; static const char *__doc_mplib_AttachedBodyTpl_getObject = R"doc( -Gets the attached object (CollisionObjectPtr))doc"; +Gets the attached object (``FCLObjectPtr``))doc"; static const char *__doc_mplib_AttachedBodyTpl_getPose = R"doc( diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 38cc0ebf..24a893bf 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -53,7 +53,15 @@ Adds an articulation (ArticulatedModelPtr) with given name to world static const char *__doc_mplib_PlanningWorldTpl_addNormalObject = R"doc( -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)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_addNormalObject_2 = +R"doc( +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)doc"; @@ -265,7 +273,7 @@ Gets the attached body (AttachedBodyPtr) with given name static const char *__doc_mplib_PlanningWorldTpl_getNormalObject = R"doc( -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 ``None`` if not found.)doc"; diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 75e36734..370e0286 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -22,6 +22,7 @@ using CollisionRequest = fcl::CollisionRequest; using DistanceRequest = fcl::DistanceRequest; using CollisionGeometryPtr = fcl::CollisionGeometryPtr; using CollisionObjectPtr = fcl::CollisionObjectPtr; +using FCLObjectPtr = collision_detection::FCLObjectPtr; void build_pyplanning_world(py::module &pymp) { auto PyPlanningWorld = py::class_>( @@ -29,11 +30,10 @@ void build_pyplanning_world(py::module &pymp) { PyPlanningWorld .def(py::init &, - const std::vector &, - const std::vector &, + const std::vector &, const std::vector &, const std::vector &>(), py::arg("articulations"), py::arg("articulation_names"), - py::arg("normal_objects") = std::vector(), + py::arg("normal_objects") = std::vector(), py::arg("normal_object_names") = std::vector(), DOC(mplib, PlanningWorldTpl, PlanningWorldTpl)) @@ -62,8 +62,16 @@ void build_pyplanning_world(py::module &pymp) { DOC(mplib, PlanningWorldTpl, getNormalObject)) .def("has_normal_object", &PlanningWorld::hasNormalObject, py::arg("name"), DOC(mplib, PlanningWorldTpl, hasNormalObject)) - .def("add_normal_object", &PlanningWorld::addNormalObject, py::arg("name"), - py::arg("collision_object"), DOC(mplib, PlanningWorldTpl, addNormalObject)) + .def("add_normal_object", + py::overload_cast( + &PlanningWorld::addNormalObject), + py::arg("name"), py::arg("collision_object"), + DOC(mplib, PlanningWorldTpl, addNormalObject)) + .def("add_normal_object", + py::overload_cast( + &PlanningWorld::addNormalObject), + py::arg("name"), py::arg("collision_object"), + DOC(mplib, PlanningWorldTpl, addNormalObject, 2)) .def("add_point_cloud", &PlanningWorld::addPointCloud, py::arg("name"), py::arg("vertices"), py::arg("resolution") = 0.01, DOC(mplib, PlanningWorldTpl, addPointCloud)) diff --git a/src/collision_detection/fcl/collision_common.cpp b/src/collision_detection/fcl/collision_common.cpp new file mode 100644 index 00000000..a7960264 --- /dev/null +++ b/src/collision_detection/fcl/collision_common.cpp @@ -0,0 +1,73 @@ +#include "mplib/collision_detection/fcl/collision_common.h" + +#include "mplib/macros/assert.h" + +namespace mplib::collision_detection::fcl { + +// Explicit Template Instantiation Definition ========================================== +#define DEFINE_TEMPLATE_FCL_COMMON(S) \ + template struct FCLObject; \ + template size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ + const fcl::CollisionRequest &request, \ + fcl::CollisionResult &result); \ + template S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ + const fcl::DistanceRequest &request, \ + fcl::DistanceResult &result) + +DEFINE_TEMPLATE_FCL_COMMON(float); +DEFINE_TEMPLATE_FCL_COMMON(double); + +template +FCLObject::FCLObject(const std::string &name, const Isometry3 &pose, + const std::vector> &shapes, + const std::vector> &shape_poses) + : name(name), pose(pose), shapes(shapes), shape_poses(shape_poses) { + ASSERT(shapes.size() == shape_poses.size(), + "shapes and shape_poses should have the same size"); + // Update pose of the shapes + for (size_t i = 0; i < shapes.size(); i++) + shapes[i]->setTransform(pose * shape_poses[i]); +} + +template +size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const fcl::CollisionRequest &request, + fcl::CollisionResult &result) { + // TODO: can request.enable_contact and request.enable_cost be used to short-circuit? + for (const auto &shape1 : obj1->shapes) + for (const auto &shape2 : obj2->shapes) { + if (request.isSatisfied(result)) return result.numContacts(); + + fcl::CollisionResult tmp_result; + fcl::collide(shape1.get(), shape2.get(), request, tmp_result); + + for (size_t i = 0; i < tmp_result.numContacts(); i++) + result.addContact(tmp_result.getContact(i)); + + auto cost_sources = std::vector>(); + tmp_result.getCostSources(cost_sources); + for (const auto &cost_source : cost_sources) + result.addCostSource(cost_source, request.num_max_cost_sources); + } + + return result.numContacts(); +} + +template +S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, + const fcl::DistanceRequest &request, fcl::DistanceResult &result) { + // TODO: can request.enable_nearest_points be used to short-circuit? + for (const auto &shape1 : obj1->shapes) + for (const auto &shape2 : obj2->shapes) { + if (request.isSatisfied(result)) return result.min_distance; + + fcl::DistanceResult tmp_result; + fcl::distance(shape1.get(), shape2.get(), request, tmp_result); + + result.update(tmp_result); + } + + return result.min_distance; +} + +} // namespace mplib::collision_detection::fcl diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 17bfca2f..fd3e8431 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -42,25 +43,25 @@ FCLModelTpl::FCLModelTpl(const urdf::ModelInterfaceSharedPtr &urdf_model, template std::unique_ptr> FCLModelTpl::createFromURDFString( const std::string &urdf_string, - const std::vector>>> - &collision_links, + const std::vector>> &collision_links, bool verbose) { auto urdf = urdf::parseURDF(urdf_string); // package_dir is not needed since urdf_string contains no visual/collision elements auto fcl_model = std::make_unique>(urdf, "", verbose, false); - for (const auto &[link_name, collision_objs] : collision_links) - for (const auto &collision_obj : collision_objs) { - fcl_model->collision_objects_.push_back(collision_obj); - fcl_model->collision_link_names_.push_back(link_name); - // fcl_model->parent_link_names_.push_back(parent_link_name); // FIXME: remove - fcl_model->collision_origin2link_poses_.push_back(collision_obj->getTransform()); - } + for (const auto &[link_name, collision_obj] : collision_links) { + fcl_model->collision_objects_.push_back(collision_obj); + fcl_model->collision_link_names_.push_back(link_name); + } + + // TODO: this should not be needed after switching to FCLObject // setLinkOrder with unique collision link names as user_link_names auto user_link_names = fcl_model->collision_link_names_; auto last = std::unique(user_link_names.begin(), user_link_names.end()); - user_link_names.erase(last, user_link_names.end()); - fcl_model->setLinkOrder(user_link_names); + if (last != user_link_names.end()) + throw std::runtime_error("URDF link names are not unique."); + // user_link_names.erase(last, user_link_names.end()); + fcl_model->setLinkOrder(fcl_model->collision_link_names_); // We assume that the collisions between objects on the same link can be ignored. for (size_t i = 0; i < fcl_model->collision_link_names_.size(); i++) @@ -80,31 +81,32 @@ void FCLModelTpl::init(const urdf::ModelInterfaceSharedPtr &urdf_model, throw std::invalid_argument("The XML stream does not contain a valid URDF model."); urdf::LinkConstSharedPtr root_link = urdf_model_->getRoot(); dfsParseTree(root_link, "root's parent"); + + // TODO: this should not be needed after switching to FCLObject // setLinkOrder with unique collision link names as user_link_names auto user_link_names = collision_link_names_; auto last = std::unique(user_link_names.begin(), user_link_names.end()); - user_link_names.erase(last, user_link_names.end()); - setLinkOrder(user_link_names); + if (last != user_link_names.end()) + throw std::runtime_error("URDF link names are not unique."); + // user_link_names.erase(last, user_link_names.end()); + setLinkOrder(collision_link_names_); for (size_t i = 0; i < collision_link_names_.size(); i++) for (size_t j = 0; j < i; j++) - if (collision_link_names_[i] != collision_link_names_[j] && - parent_link_names_[i] != collision_link_names_[j] && - parent_link_names_[j] != collision_link_names_[i]) { - // We assume that the collisions between objects append to the same joint can be - // ignored. + if (collision_link_names_[i] != collision_link_names_[j]) collision_pairs_.push_back(std::make_pair(j, i)); - } } template void FCLModelTpl::dfsParseTree(const urdf::LinkConstSharedPtr &link, const std::string &parent_link_name) { - if (link->collision) + if (link->collision) { + auto fcl_obj = std::make_shared>(link->name); + fcl_obj->pose = Isometry3::Identity(); + for (const auto &geom : link->collision_array) { auto geom_model = geom->geometry; fcl::CollisionGeometryPtr collision_geometry; - auto pose = Isometry3::Identity(); if (geom_model->type == urdf::Geometry::MESH) { const urdf::MeshSharedPtr urdf_mesh = urdf::dynamic_pointer_cast(geom_model); @@ -147,13 +149,15 @@ void FCLModelTpl::dfsParseTree(const urdf::LinkConstSharedPtr &link, if (!collision_geometry) throw std::invalid_argument("The polyhedron retrived is empty"); - auto obj {std::make_shared>(collision_geometry, pose)}; - collision_objects_.push_back(obj); - collision_link_names_.push_back(link->name); - parent_link_names_.push_back(parent_link_name); - collision_origin2link_poses_.push_back(toIsometry(geom->origin)); + fcl_obj->shapes.push_back(std::make_shared>( + collision_geometry, Isometry3::Identity())); + fcl_obj->shape_poses.push_back(toIsometry(geom->origin)); } + + collision_objects_.push_back(fcl_obj); + collision_link_names_.push_back(link->name); + } for (const auto &child : link->child_links) dfsParseTree(child, link->name); } @@ -221,30 +225,34 @@ void FCLModelTpl::removeCollisionPairsFromSRDFString( template void FCLModelTpl::updateCollisionObjects( - const std::vector> &link_pose) const { + const std::vector> &link_poses) const { for (size_t i = 0; i < collision_objects_.size(); i++) { - auto link_i = collision_link_user_indices_[i]; - collision_objects_[i]->setTransform(toIsometry(link_pose[link_i]) * - collision_origin2link_poses_[i]); + const auto link_pose = toIsometry(link_poses[collision_link_user_indices_[i]]); + const auto &fcl_obj = collision_objects_[i]; + fcl_obj->pose = link_pose; + for (size_t j = 0; j < fcl_obj->shapes.size(); j++) + fcl_obj->shapes[j]->setTransform(link_pose * fcl_obj->shape_poses[j]); } } template void FCLModelTpl::updateCollisionObjects( - const std::vector> &link_pose) const { + const std::vector> &link_poses) const { for (size_t i = 0; i < collision_objects_.size(); i++) { - auto link_i = collision_link_user_indices_[i]; - collision_objects_[i]->setTransform(link_pose[link_i] * - collision_origin2link_poses_[i]); + const auto link_pose = link_poses[collision_link_user_indices_[i]]; + const auto &fcl_obj = collision_objects_[i]; + fcl_obj->pose = link_pose; + for (size_t j = 0; j < fcl_obj->shapes.size(); j++) + fcl_obj->shapes[j]->setTransform(link_pose * fcl_obj->shape_poses[j]); } } template bool FCLModelTpl::collide(const fcl::CollisionRequest &request) const { fcl::CollisionResult result; - for (const auto &col_pair : collision_pairs_) { - fcl::collide(collision_objects_[col_pair.first].get(), - collision_objects_[col_pair.second].get(), request, result); + for (const auto &[i, j] : collision_pairs_) { + collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], + request, result); if (result.isCollision()) return true; } return false; @@ -253,12 +261,13 @@ bool FCLModelTpl::collide(const fcl::CollisionRequest &request) const { template std::vector> FCLModelTpl::collideFull( const fcl::CollisionRequest &request) const { + // TODO(merge): return only CollisionResult in collision? // Result will be returned via the collision result structure std::vector> ret; - for (const auto &col_pair : collision_pairs_) { + for (const auto &[i, j] : collision_pairs_) { fcl::CollisionResult result; - fcl::collide(collision_objects_[col_pair.first].get(), - collision_objects_[col_pair.second].get(), request, result); + collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], + request, result); ret.push_back(result); } return ret; diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index d3cf9cc9..de67ec32 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -42,7 +42,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, template std::unique_ptr> ArticulatedModelTpl::createFromURDFString( const std::string &urdf_string, const std::string &srdf_string, - const std::vector>>> + const std::vector>> &collision_links, const Vector3 &gravity, const std::vector &link_names, const std::vector &joint_names, bool verbose) { diff --git a/src/core/attached_body.cpp b/src/core/attached_body.cpp index d06bde72..9254ce53 100644 --- a/src/core/attached_body.cpp +++ b/src/core/attached_body.cpp @@ -9,8 +9,7 @@ DEFINE_TEMPLATE_ATTACHED_BODY(float); DEFINE_TEMPLATE_ATTACHED_BODY(double); template -AttachedBodyTpl::AttachedBodyTpl(const std::string &name, - const CollisionObjectPtr &object, +AttachedBodyTpl::AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object, const ArticulatedModelPtr &attached_articulation, int attached_link_id, const Isometry3 &pose, const std::vector &touch_links) @@ -24,4 +23,12 @@ AttachedBodyTpl::AttachedBodyTpl(const std::string &name, updatePose(); // updates global pose using link_pose and attached_pose } +template +void AttachedBodyTpl::updatePose() const { + auto object_pose = getGlobalPose(); + object_->pose = object_pose; + for (size_t i = 0; i < object_->shapes.size(); i++) + object_->shapes[i]->setTransform(object_pose * object_->shape_poses[i]); +} + } // namespace mplib diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 10f62b69..391b625d 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -19,22 +19,21 @@ template PlanningWorldTpl::PlanningWorldTpl( const std::vector &articulations, const std::vector &articulation_names, - const std::vector &normal_objects, + const std::vector &normal_objects, const std::vector &normal_object_names) : acm_(std::make_shared()) { ASSERT(articulations.size() == articulation_names.size(), "articulations and articulation_names should have the same size"); ASSERT(normal_objects.size() == normal_object_names.size(), "normal_objects and normal_object_names should have the same size"); - // TODO(merge): remove articulation_names, and normal_object_names + // TODO(merge): remove articulation_names and normal_object_names for (size_t i = 0; i < articulations.size(); i++) { articulations[i]->setName(articulation_names[i]); articulation_map_[articulation_names[i]] = articulations[i]; planned_articulation_map_[articulation_names[i]] = articulations[i]; } - for (size_t i = 0; i < normal_objects.size(); i++) { + for (size_t i = 0; i < normal_objects.size(); i++) normal_object_map_[normal_object_names[i]] = normal_objects[i]; - } } template @@ -91,6 +90,15 @@ std::vector PlanningWorldTpl::getNormalObjectNames() const { return names; } +template +void PlanningWorldTpl::addNormalObject(const std::string &name, + const CollisionObjectPtr &collision_object) { + addNormalObject(name, std::make_shared( + name, collision_object->getTransform(), + std::vector {collision_object}, + std::vector> {Isometry3::Identity()})); +} + template void PlanningWorldTpl::addPointCloud(const std::string &name, const MatrixX3 &vertices, @@ -98,8 +106,8 @@ void PlanningWorldTpl::addPointCloud(const std::string &name, auto tree = std::make_shared(resolution); for (const auto &row : vertices.rowwise()) tree->updateNode(octomap::point3d(row(0), row(1), row(2)), true); - auto obj = std::make_shared(std::make_shared>(tree)); - addNormalObject(name, obj); + addNormalObject( + name, std::make_shared(std::make_shared>(tree))); } template @@ -117,7 +125,7 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const std::vector &touch_links) { - const auto T_world_obj = normal_object_map_.at(name)->getTransform(); + const auto T_world_obj = normal_object_map_.at(name)->pose; const auto T_world_link = toIsometry( planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose( link_id)); @@ -128,7 +136,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id) { - const auto T_world_obj = normal_object_map_.at(name)->getTransform(); + const auto T_world_obj = normal_object_map_.at(name)->pose; const auto T_world_link = toIsometry( planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose( link_id)); @@ -226,6 +234,7 @@ template void PlanningWorldTpl::attachMesh(const std::string &mesh_path, const std::string &art_name, int link_id, const Vector7 &pose) { + // TODO(merge): Convex mesh? // FIXME: Use link_name to avoid changes auto name = art_name + "_" + std::to_string(link_id) + "_mesh"; attachObject( @@ -319,43 +328,39 @@ std::vector> PlanningWorldTpl::selfCollide( ret.push_back(tmp); } - // Collision among planned_articulations_ + // Collision among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) break; - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs.size(); i++) - for (size_t j = 0; j < col_objs2.size(); j++) { + for (const auto &col_obj : col_objs) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { result.clear(); - ::fcl::collide(col_objs[i].get(), col_objs2[j].get(), request, result); + collision_detection::fcl::collide(col_obj, col_obj2, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "self_articulation"; tmp.object_name1 = art_name; tmp.object_name2 = art_name2; - tmp.link_name1 = col_link_names[i]; - tmp.link_name2 = col_link_names2[j]; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = col_obj2->name; ret.push_back(tmp); } } } - // Articulation collide with attached_bodies_ + // Articulation collide with attached_body_map_ for (const auto &[attached_body_name, attached_body] : attached_body_map_) { - auto attached_obj = attached_body->getObject(); - for (size_t i = 0; i < col_objs.size(); i++) { + const auto attached_obj = attached_body->getObject(); + for (const auto &col_obj : col_objs) { result.clear(); - ::fcl::collide(attached_obj.get(), col_objs[i].get(), request, result); + collision_detection::fcl::collide(attached_obj, col_obj, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "self_attach"; tmp.object_name1 = art_name; tmp.object_name2 = attached_body_name; - tmp.link_name1 = col_link_names[i]; + tmp.link_name1 = col_obj->name; tmp.link_name2 = attached_body_name; ret.push_back(tmp); } @@ -363,12 +368,12 @@ std::vector> PlanningWorldTpl::selfCollide( } } - // Collision among attached_bodies_ + // Collision among attached_body_map_ for (auto it = attached_body_map_.begin(); it != attached_body_map_.end(); ++it) for (auto it2 = attached_body_map_.begin(); it2 != it; ++it2) { result.clear(); - ::fcl::collide(it->second->getObject().get(), it2->second->getObject().get(), - request, result); + collision_detection::fcl::collide(it->second->getObject(), + it2->second->getObject(), request, result); if (result.isCollision()) { auto name1 = it->first, name2 = it2->first; WorldCollisionResult tmp; @@ -394,101 +399,87 @@ std::vector> PlanningWorldTpl::collideWithOthers( // Collect unplanned articulations, not attached scene objects std::vector unplanned_articulations; - std::unordered_map scene_objects; + std::vector scene_objects; for (const auto &[name, art] : articulation_map_) if (planned_articulation_map_.find(name) == planned_articulation_map_.end()) unplanned_articulations.push_back(art); for (const auto &[name, obj] : normal_object_map_) if (attached_body_map_.find(name) == attached_body_map_.end()) - scene_objects[name] = obj; + scene_objects.push_back(obj); // Collision involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); auto col_objs = fcl_model->getCollisionObjects(); - auto col_link_names = fcl_model->getCollisionLinkNames(); // Collision with unplanned articulation - for (const auto &art2 : unplanned_articulations) { - auto art_name2 = art2->getName(); - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs.size(); i++) - for (size_t j = 0; j < col_objs2.size(); j++) { + for (const auto &art2 : unplanned_articulations) + for (const auto &col_obj : col_objs) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { result.clear(); - ::fcl::collide(col_objs[i].get(), col_objs2[j].get(), request, result); + collision_detection::fcl::collide(col_obj, col_obj2, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "articulation_articulation"; tmp.object_name1 = art_name; - tmp.object_name2 = art_name2; - tmp.link_name1 = col_link_names[i]; - tmp.link_name2 = col_link_names2[j]; + tmp.object_name2 = art2->getName(); + tmp.link_name1 = col_obj->name; + tmp.link_name2 = col_obj2->name; ret.push_back(tmp); } } - } // Collision with scene objects - for (const auto &[name, obj] : scene_objects) - for (size_t i = 0; i < col_objs.size(); i++) { + for (const auto &scene_obj : scene_objects) + for (const auto &col_obj : col_objs) { result.clear(); - ::fcl::collide(col_objs[i].get(), obj.get(), request, result); + collision_detection::fcl::collide(col_obj, scene_obj, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "articulation_sceneobject"; tmp.object_name1 = art_name; - tmp.object_name2 = name; - tmp.link_name1 = col_link_names[i]; - tmp.link_name2 = name; + tmp.object_name2 = scene_obj->name; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = scene_obj->name; ret.push_back(tmp); } } } - // Collision involving attached_bodies_ + // Collision involving attached_body_map_ for (const auto &[attached_body_name, attached_body] : attached_body_map_) { - auto attached_obj = attached_body->getObject(); - + const auto attached_obj = attached_body->getObject(); // Collision with unplanned articulation - for (const auto &art2 : unplanned_articulations) { - auto art_name2 = art2->getName(); - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs2.size(); i++) { + for (const auto &art2 : unplanned_articulations) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { result.clear(); - ::fcl::collide(attached_obj.get(), col_objs2[i].get(), request, result); + collision_detection::fcl::collide(attached_obj, col_obj2, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "attach_articulation"; tmp.object_name1 = attached_body_name; - tmp.object_name2 = art_name2; + tmp.object_name2 = art2->getName(); tmp.link_name1 = attached_body_name; - tmp.link_name2 = col_link_names2[i]; + tmp.link_name2 = col_obj2->name; ret.push_back(tmp); } } - } // Collision with scene objects - for (const auto &[name, obj] : scene_objects) { + for (const auto &scene_obj : scene_objects) { result.clear(); - ::fcl::collide(attached_obj.get(), obj.get(), request, result); + collision_detection::fcl::collide(attached_obj, scene_obj, request, result); if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; tmp.collision_type = "attach_sceneobject"; tmp.object_name1 = attached_body_name; - tmp.object_name2 = name; + tmp.object_name2 = scene_obj->name; tmp.link_name1 = attached_body_name; - tmp.link_name2 = name; + tmp.link_name2 = scene_obj->name; ret.push_back(tmp); } } @@ -500,7 +491,7 @@ template std::vector> PlanningWorldTpl::collideFull( const CollisionRequest &request) const { auto ret1 = selfCollide(request); - auto ret2 = collideWithOthers(request); + const auto ret2 = collideWithOthers(request); ret1.insert(ret1.end(), ret2.begin(), ret2.end()); return ret1; } @@ -520,12 +511,13 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( auto col_link_names = fcl_model->getCollisionLinkNames(); auto col_pairs = fcl_model->getCollisionPairs(); + // TODO(merge): add FCLModel::distanceSelf() method // Articulation minimum distance to self-collision for (const auto &[x, y] : col_pairs) if (auto type = acm_->getAllowedCollision(col_link_names[x], col_link_names[y]); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(col_objs[x].get(), col_objs[y].get(), request, result); + collision_detection::fcl::distance(col_objs[x], col_objs[y], request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; @@ -537,28 +529,23 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( } } - // Minimum distance among planned_articulations_ + // Minimum distance among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) break; - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs.size(); i++) - for (size_t j = 0; j < col_objs2.size(); j++) - if (auto type = - acm_->getAllowedCollision(col_link_names[i], col_link_names2[j]); + for (const auto &col_obj : col_objs) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) + if (auto type = acm_->getAllowedCollision(col_obj->name, col_obj2->name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(col_objs[i].get(), col_objs2[j].get(), request, result); + collision_detection::fcl::distance(col_obj, col_obj2, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "self_articulation"; ret.object_name1 = art_name; ret.object_name2 = art_name2; - ret.link_name1 = col_link_names[i]; - ret.link_name2 = col_link_names2[j]; + ret.link_name1 = col_obj->name; + ret.link_name2 = col_obj2->name; } } } @@ -566,23 +553,21 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( // Articulation minimum distance to attached_body_map_ for (const auto &[attached_body_name, attached_body] : attached_body_map_) { auto attached_obj = attached_body->getObject(); - for (size_t i = 0; i < col_objs.size(); i++) { - if (auto type = - acm_->getAllowedCollision(col_link_names[i], attached_body_name); + for (const auto &col_obj : col_objs) + if (auto type = acm_->getAllowedCollision(col_obj->name, attached_body_name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(attached_obj.get(), col_objs[i].get(), request, result); + collision_detection::fcl::distance(attached_obj, col_obj, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "self_attach"; ret.object_name1 = art_name; ret.object_name2 = attached_body_name; - ret.link_name1 = col_link_names[i]; + ret.link_name1 = col_obj->name; ret.link_name2 = attached_body_name; } } - } } } @@ -593,8 +578,8 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( if (auto type = acm_->getAllowedCollision(name1, name2); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(it->second->getObject().get(), it2->second->getObject().get(), - request, result); + collision_detection::fcl::distance(it->second->getObject(), + it2->second->getObject(), request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; @@ -619,108 +604,92 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceOthers( // Collect unplanned articulations, not attached scene objects std::vector unplanned_articulations; - std::unordered_map scene_objects; + std::vector scene_objects; for (const auto &[name, art] : articulation_map_) if (planned_articulation_map_.find(name) == planned_articulation_map_.end()) unplanned_articulations.push_back(art); for (const auto &[name, obj] : normal_object_map_) if (attached_body_map_.find(name) == attached_body_map_.end()) - scene_objects[name] = obj; + scene_objects.push_back(obj); // Minimum distance involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); auto col_objs = fcl_model->getCollisionObjects(); - auto col_link_names = fcl_model->getCollisionLinkNames(); // Minimum distance to unplanned articulation - for (const auto &art2 : unplanned_articulations) { - auto art_name2 = art2->getName(); - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs.size(); i++) - for (size_t j = 0; j < col_objs2.size(); j++) - if (auto type = - acm_->getAllowedCollision(col_link_names[i], col_link_names2[j]); + for (const auto &art2 : unplanned_articulations) + for (const auto &col_obj : col_objs) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) + if (auto type = acm_->getAllowedCollision(col_obj->name, col_obj2->name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(col_objs[i].get(), col_objs2[j].get(), request, result); + collision_detection::fcl::distance(col_obj, col_obj2, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "articulation_articulation"; ret.object_name1 = art_name; - ret.object_name2 = art_name2; - ret.link_name1 = col_link_names[i]; - ret.link_name2 = col_link_names2[j]; + ret.object_name2 = art2->getName(); + ret.link_name1 = col_obj->name; + ret.link_name2 = col_obj2->name; } } - } // Minimum distance to scene objects - for (const auto &[name, obj] : scene_objects) - for (size_t i = 0; i < col_objs.size(); i++) - if (auto type = acm_->getAllowedCollision(col_link_names[i], name); + for (const auto &scene_obj : scene_objects) + for (const auto &col_obj : col_objs) + if (auto type = acm_->getAllowedCollision(col_obj->name, scene_obj->name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(col_objs[i].get(), obj.get(), request, result); + collision_detection::fcl::distance(col_obj, scene_obj, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "articulation_sceneobject"; ret.object_name1 = art_name; - ret.object_name2 = name; - ret.link_name1 = col_link_names[i]; - ret.link_name2 = name; + ret.object_name2 = scene_obj->name; + ret.link_name1 = col_obj->name; + ret.link_name2 = scene_obj->name; } } } // Minimum distance involving attached_body_map_ for (const auto &[attached_body_name, attached_body] : attached_body_map_) { - auto attached_obj = attached_body->getObject(); - + const auto attached_obj = attached_body->getObject(); // Minimum distance to unplanned articulation - for (const auto &art2 : unplanned_articulations) { - auto art_name2 = art2->getName(); - auto fcl_model2 = art2->getFCLModel(); - auto col_objs2 = fcl_model2->getCollisionObjects(); - auto col_link_names2 = fcl_model2->getCollisionLinkNames(); - - for (size_t i = 0; i < col_objs2.size(); i++) - if (auto type = - acm_->getAllowedCollision(attached_body_name, col_link_names2[i]); + for (const auto &art2 : unplanned_articulations) + for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) + if (auto type = acm_->getAllowedCollision(attached_body_name, col_obj2->name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(attached_obj.get(), col_objs2[i].get(), request, result); + collision_detection::fcl::distance(attached_obj, col_obj2, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "attach_articulation"; ret.object_name1 = attached_body_name; - ret.object_name2 = art_name2; + ret.object_name2 = art2->getName(); ret.link_name1 = attached_body_name; - ret.link_name2 = col_link_names2[i]; + ret.link_name2 = col_obj2->name; } } - } // Minimum distance to scene objects - for (const auto &[name, obj] : scene_objects) - if (auto type = acm_->getAllowedCollision(attached_body_name, name); + for (const auto &scene_obj : scene_objects) + if (auto type = acm_->getAllowedCollision(attached_body_name, scene_obj->name); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); - ::fcl::distance(attached_obj.get(), obj.get(), request, result); + collision_detection::fcl::distance(attached_obj, scene_obj, request, result); if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; ret.distance_type = "attach_sceneobject"; ret.object_name1 = attached_body_name; - ret.object_name2 = name; + ret.object_name2 = scene_obj->name; ret.link_name1 = attached_body_name; - ret.link_name2 = name; + ret.link_name2 = scene_obj->name; } } } @@ -730,8 +699,8 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceOthers( template WorldDistanceResultTpl PlanningWorldTpl::distanceFull( const DistanceRequest &request) const { - auto ret1 = distanceSelf(request); - auto ret2 = distanceOthers(request); + const auto ret1 = distanceSelf(request); + const auto ret2 = distanceOthers(request); return ret1.min_distance < ret2.min_distance ? ret1 : ret2; } diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 33172ffa..96123fa4 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -40,7 +40,9 @@ 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].shapes[0].set_transformation( + link_pose + ) def test_get_collision_objects(self): self.assertEqual( @@ -48,8 +50,10 @@ def test_get_collision_objects(self): ) 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].shapes[0].get_translation() + quat = mat2quat( + self.model.get_collision_objects()[i].shapes[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)