Fix CheckLinkSelfCollision and CheckEndEffectorCollision

include/openrave/collisionchecker.h

@@ -321,8 +321,9 @@ class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
     /// \brief Checks self collision of the link with the rest of the links with its parent
     ///
     /// Only checks KinBody::GetNonAdjacentLinks(), Links that are joined together are ignored.
+    /// \param[in] plink, vIncludedLinks : The pointer of target link and vector of included links. Among the link pairs in GetNonAdjacentLinks, this API only checks the collisions with the pairs so that one of the link of the pair is plink, and the other link of the pair is included in vIncludedLinks. If vIncludedLinks is empty, it's specially treated as no limitation for the included links, e.g. this API checks all possible pairs if one link of the pair is plink.
     /// \param[out] report [optional] collision report to be filled with data about the collision.
-    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) = 0;
+    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr()) = 0;
 
     /// \deprecated (13/04/09)
     virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportPtr report = CollisionReportPtr()) RAVE_DEPRECATED
@@ -335,7 +336,7 @@ class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
     virtual bool CheckSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) RAVE_DEPRECATED
     {
         //RAVELOG_WARN("CollisionCheckerBase::CheckSelfCollision has been deprecated, please use CollisionCheckerBase::CheckStandaloneSelfCollision\n");
-        return CheckStandaloneSelfCollision(plink,report);
+        return CheckStandaloneSelfCollision(plink, std::vector<KinBody::LinkConstPtr>(), report);
     }
 
 protected:
@@ -351,6 +352,33 @@ class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
         return boost::static_pointer_cast<CollisionCheckerBase const>(shared_from_this());
     }
 
+    /// \brief check if the caller should skip the given standalone self collision check pair.
+    /// \param[in] targetLink, vIncludedLinks : target link to check and vector of included links.
+    ///                                         - if index1 and index2 are not same as targetLink's index, should skip.
+    ///                                         - if vIncludedLinks is empty, no check for another link in the pair and the given pair should be checked with collision check.
+    ///                                         - if vIncludedLinks is not empty, if another link in the pair is not included in vIncludedLinks, such pair should be skipped with collision check.
+    /// \param[in] index1, index2 : link indices of the pair.
+    /// \param[in] pBody : kinbody to check self collision
+    /// \return false if the given pair (index1 and index2) should be checked with standalone self collision checking. true if it should be skipped.
+    static inline bool _ShouldSkipStandaloneSelfCollisionCheckPair(const KinBody::Link& taretLink, const size_t index1, const size_t index2, const KinBodyPtr& pBody, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks)
+    {
+        const size_t targetLinkIndex = taretLink.GetIndex();
+        if( targetLinkIndex == index1 ) {
+            if( vIncludedLinks.size() > 0 && std::find(vIncludedLinks.begin(), vIncludedLinks.end(), pBody->GetLinks().at(index2)) == vIncludedLinks.end() ) {
+                return true;
+            }
+        }
+        else if( targetLinkIndex == index2 ) {
+            if( vIncludedLinks.size() > 0 && std::find(vIncludedLinks.begin(), vIncludedLinks.end(), pBody->GetLinks().at(index1)) == vIncludedLinks.end() ) {
+                return true;
+            }
+        }
+        else {
+            return true;
+        }
+        return false;
+    }
+
 private:
     virtual const char* GetHash() const override {
         return OPENRAVE_COLLISIONCHECKER_HASH;

include/openrave/kinbody.h

@@ -3255,17 +3255,12 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /** \brief checks self-collision of a robot link with the other robot links. Attached/Grabbed bodies to this link are also checked for self-collision. Rigidly attached links to the specified link are not checked for self-collision.
 
         \param[in] ilinkindex the index of the link to check
+        \param[in] vIncludedLinks vector of included links.
+                   Among the link pairs in GetNonAdjacentLinks, this API only checks the collisions with the pairs so that one of the link of the pair has ilinkindex, and the other link of the pair is included in vIncludedLinks. If vIncludedLinks is empty, it's specially treated as no limitation for the included links, e.g. this API checks all possible pairs if one link of the pair has ilinkindex.
+        \param[in] pLinkTransform The pointer of the transform of the link to check. If nullptr, do nothing about link transform. If valid pointer is specified, update the link transform and grabbed bodies transforms according to the pLinkTransform.
         \param[out] report [optional] collision report
      */
-    virtual bool CheckLinkSelfCollision(int ilinkindex, CollisionReportPtr report = CollisionReportPtr());
-
-    /** \brief checks self-collision of a robot link with the other robot links. Attached/Grabbed bodies to this link are also checked for self-collision. Rigidly attached links to the specified link are not checked for self-collision.
-
-        \param[in] ilinkindex the index of the link to check
-        \param[in] tlinktrans The transform of the link to check
-        \param[out] report [optional] collision report
-     */
-    virtual bool CheckLinkSelfCollision(int ilinkindex, const Transform& tlinktrans, CollisionReportPtr report = CollisionReportPtr());
+    virtual bool CheckLinkSelfCollision(int ilinkindex, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks, const TransformConstPtr& pLinkTransform, CollisionReportPtr report = CollisionReportPtr());
 
     //@}
 
@@ -3775,6 +3770,37 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \brief Extract the first body's environmentBodyIndex from environment body indices pair.
     static int _GetSecondEnvironmentBodyIndexFromPair(const uint64_t pair);
 
+    /// \brief Check self collision for grabbed bodies: 1) check between grabber and grabbed, and 2) check between two grabbed bodies (inter-grabbed collision).
+    ///
+    /// \param collisionchecker : collision checker to use
+    /// \param[out] report : resultant report
+    /// \param[in] bAllLinkCollisions : true if all link should be checked.
+    /// \param[in] pGrabberLinkToCheck : link ptr of grabber's link to check.
+    ///                                  If nullptr, check all possible grabbed bodies.
+    ///                                  If valid ptr is specified,
+    ///                                     - grabber vs grabbed collision
+    ///                                       - check collision between grabber link and the grabbed body which pGrabbingLink is pGrabberLinkToCheck
+    ///                                       - check collision between the grabbed body and the grabber link which is pGrabberLinkToCheck
+    ///                                     - inter-grabbed collision
+    ///                                       - check collision if pGrabbingLink of one grabbed body is pGrabberLinkToCheck.
+    /// \param[in] vIncludedLinks : Vector of grabber links to check. This argument requires pGrabberLinkToCheck to be specified.
+    ///                                    If empty, vIncludedLinks is not used and check all possible grabber links.
+    ///                                    If non-empty, vIncludedLinks is used along with pGrabberLinkToCheck as follows:
+    ///                                     - grabber vs grabbed collision
+    ///                                       - check collision between grabber link included in vIncludedLinks and the grabbed body which pGrabbingLink is pGrabberLinkToCheck
+    ///                                       - check collision between the grabbed body which pGrabbingLink is included in vIncludedLinks and the grabber link which is pGrabberLinkToCheck
+    ///                                     - inter-grabbed collision
+    ///                                       - check collision if pGrabbingLink of one grabbed body is pGrabberLinkToCheck and pGrabbingLink of other grabbed body is included in vIncludedLinks.
+    /// \param[in] pLinkTransformForGrabberLinkToCheck : ptr of link transform for pGrabberLinkToCheck. This argument requires pGrabberLinkToCheck to be specified.
+    ///                                                  If nullptr is specified, no transform change for grabbed bodies. 
+    ///                                                  If valid ptr is specified, update the Transform of grabbed bodies, which are grabbed by pGrabberLinkToCheck
+    bool _CheckGrabbedBodiesSelfCollision(CollisionCheckerBasePtr& collisionchecker,
+                                          CollisionReportPtr& report,
+                                          const bool bAllLinkCollisions,
+                                          const LinkPtr& pGrabberLinkToCheck,
+                                          const std::vector<KinBody::LinkConstPtr>& vIncludedLinks,
+                                          const TransformConstPtr& pLinkTransformForGrabberLinkToCheck) const;
+
     std::string _name; ///< name of body
 
     std::vector<JointPtr> _vecjoints; ///< \see GetJoints

include/openrave/robot.h

@@ -516,6 +516,9 @@ class OPENRAVE_API RobotBase : public KinBody
         bool _CheckEndEffectorCollision(const Transform& tEE, KinBodyConstPtr pbody, CollisionReportPtr report) const;
         /// \brief check end-effector collision with the given ikparam and the specified body. if pbody is null, check with the environment.
         bool _CheckEndEffectorCollision(const IkParameterization& ikparam, KinBodyConstPtr pbody, CollisionReportPtr report, int numredundantsamples) const;
+        /// \brief check end effector self collision.
+        /// \param[in] pTransformDelta : The pointer of the delta of link transform. If nullptr, do nothing about link transform. If valid pointer is specified, update the link transforms temporarily.
+        bool _CheckEndEffectorSelfCollision(const TransformConstPtr& pTransformDelta, CollisionReportPtr report, bool bIgnoreManipulatorLinks) const;
 
         ManipulatorInfo _info; ///< user-set information
 private:

plugins/bulletrave/bulletcollision.h

@@ -789,7 +789,7 @@ class BulletCollisionChecker : public CollisionCheckerBase
         return bCollision;
     }
 
-    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report)
+    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr())
     {
         // dummy
         return CheckStandaloneSelfCollision(plink->GetParent(), report);

plugins/configurationcache/cachechecker.cpp

@@ -381,9 +381,9 @@ class CacheCollisionChecker : public CollisionCheckerBase
         return col;
     }
 
-    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) {
+    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr()) {
 
-        return _pintchecker->CheckStandaloneSelfCollision(plink, report);
+        return _pintchecker->CheckStandaloneSelfCollision(plink, vIncludedLinks, report);
     }
 
 protected:

plugins/fclrave/fclcollision.cpp

@@ -745,7 +745,7 @@ bool FCLCollisionChecker::CheckStandaloneSelfCollision(KinBodyConstPtr pbody, Co
     return query._bCollision;
 }
 
-bool FCLCollisionChecker::CheckStandaloneSelfCollision(LinkConstPtr plink, CollisionReportPtr report)
+bool FCLCollisionChecker::CheckStandaloneSelfCollision(LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks, CollisionReportPtr report)
 {
     START_TIMING_OPT(_statistics, "LinkSelf",_options,false);
     if( !!report ) {
@@ -779,7 +779,10 @@ bool FCLCollisionChecker::CheckStandaloneSelfCollision(LinkConstPtr plink, Colli
     FCLKinBodyInfoPtr pinfo = _fclspace->GetInfo(*pbody);
     FOREACH(itset, nonadjacent) {
         int index1 = *itset&0xffff, index2 = *itset>>16;
-        if( plink->GetIndex() == index1 || plink->GetIndex() == index2 ) {
+        if( _ShouldSkipStandaloneSelfCollisionCheckPair(*plink, index1, index2, pbody, vIncludedLinks) ) {
+            continue;
+        }
+        {
             const FCLSpace::FCLKinBodyInfo::LinkInfo& pLINK1 = *pinfo->vlinks.at(index1);
             const FCLSpace::FCLKinBodyInfo::LinkInfo& pLINK2 = *pinfo->vlinks.at(index2);
             if( !pLINK1.linkBV.second || !pLINK2.linkBV.second || !pLINK1.linkBV.second->getAABB().overlap(pLINK2.linkBV.second->getAABB()) ) {

plugins/fclrave/fclcollision.h

@@ -194,7 +194,7 @@ class FCLCollisionChecker : public OpenRAVE::CollisionCheckerBase
 
     bool CheckStandaloneSelfCollision(KinBodyConstPtr pbody, CollisionReportPtr report = CollisionReportPtr()) override;
 
-    bool CheckStandaloneSelfCollision(LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) override;
+    bool CheckStandaloneSelfCollision(LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr()) override;
 
 
 private:

plugins/oderave/odecollision.h

@@ -781,7 +781,7 @@ class ODECollisionChecker : public OpenRAVE::CollisionCheckerBase
         return bCollision;
     }
 
-    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report)
+    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr())
     {
         if( _options & OpenRAVE::CO_Distance ) {
             RAVELOG_WARN("ode doesn't support CO_Distance\n");
@@ -813,8 +813,12 @@ class ODECollisionChecker : public OpenRAVE::CollisionCheckerBase
         _odespace->Synchronize(); // call after GetNonAdjacentLinks since it can modify the body, even though it is const!
         bool bCollision = false;
         FOREACHC(itset, nonadjacent) {
-            KinBody::LinkConstPtr plink1(pbody->GetLinks().at(*itset&0xffff)), plink2(pbody->GetLinks().at(*itset>>16));
-            if( plink == plink1 || plink == plink2 ) {
+            const int index1 = *itset&0xffff, index2 = *itset>>16;
+            KinBody::LinkConstPtr plink1(pbody->GetLinks().at(index1)), plink2(pbody->GetLinks().at(index2));
+            if( _ShouldSkipStandaloneSelfCollisionCheckPair(*plink, index1, index2, pbody, vIncludedLinks) ) {
+                continue;
+            }
+            {
                 if( _CheckCollision(plink1,plink2, report) ) {
                     if( IS_DEBUGLEVEL(OpenRAVE::Level_Verbose) ) {
                         RAVELOG_VERBOSE(str(boost::format("selfcol %s, Links %s %s are colliding\n")%pbody->GetName()%plink1->GetName()%plink2->GetName()));

plugins/pqprave/collisionPQP.h

@@ -385,7 +385,7 @@ class CollisionCheckerPQP : public CollisionCheckerBase
         return false;
     }
 
-    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report)
+    virtual bool CheckStandaloneSelfCollision(KinBody::LinkConstPtr plink, const std::vector<KinBody::LinkConstPtr>& vIncludedLinks = std::vector<KinBody::LinkConstPtr>(), CollisionReportPtr report = CollisionReportPtr())
     {
         KinBodyPtr pbody = plink->GetParent();
         if( pbody->GetLinks().size() <= 1 ) {
@@ -402,8 +402,12 @@ class CollisionCheckerPQP : public CollisionCheckerBase
         const std::vector<int>& nonadjacent = pbody->GetNonAdjacentLinks(adjacentoptions);
         PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3];
         FOREACHC(itset, nonadjacent) {
-            KinBody::LinkConstPtr plink1(pbody->GetLinks().at(*itset&0xffff)), plink2(pbody->GetLinks().at(*itset>>16));
-            if( plink == plink1 || plink == plink2 ) {
+            const int index1 = *itset&0xffff, index2 = *itset>>16;
+            KinBody::LinkConstPtr plink1(pbody->GetLinks().at(index1)), plink2(pbody->GetLinks().at(index2));
+            if( _ShouldSkipStandaloneSelfCollisionCheckPair(*plink, index1, index2, pbody, vIncludedLinks) ) {
+                continue;
+            }
+            {
                 GetPQPTransformFromTransform(plink1->GetTransform(),R1,T1);
                 GetPQPTransformFromTransform(plink2->GetTransform(),R2,T2);
                 if( DoPQP(plink1,R1,T1,plink2,R2,T2,report) ) {

python/bindings/include/openravepy/openravepy_robotbase.h

@@ -444,7 +444,7 @@ class OPENRAVEPY_API PyRobotBase : public PyKinBody
     bool Grab(PyKinBodyPtr pbody, object pylink_or_linkstoignore);
     bool Grab(PyKinBodyPtr pbody, object pylink, object linkstoignore, object grabbedUserData);
 
-    bool CheckLinkSelfCollision(int ilinkindex, object olinktrans, PyCollisionReportPtr pyreport=PyCollisionReportPtr());
+    bool CheckLinkSelfCollision(int ilinkindex, object oincludedlinks, object olinktrans, PyCollisionReportPtr pyreport=PyCollisionReportPtr());
 
     bool WaitForController(float ftimeout);
 

python/bindings/openravepy_robot.cpp

@@ -2219,15 +2219,26 @@ bool PyRobotBase::Grab(PyKinBodyPtr pbody, object pylink, object linkstoignore,
     return _pbody->Grab(pbody->GetBody(), GetKinBodyLink(pylink), setlinkstoignoreInt, rGrabbedUserData);
 }
 
-bool PyRobotBase::CheckLinkSelfCollision(int ilinkindex, object olinktrans, PyCollisionReportPtr pyreport)
+bool PyRobotBase::CheckLinkSelfCollision(int ilinkindex, object oincludedlinks, object olinktrans, PyCollisionReportPtr pyreport)
 {
     CollisionReport report;
     CollisionReportPtr preport;
     if( !!pyreport ) {
         preport = CollisionReportPtr(&report,utils::null_deleter());
     }
+    std::vector<KinBody::LinkConstPtr> vIncludedLinks;
+    for(size_t iLink = 0; iLink < (size_t)len(oincludedlinks); ++iLink) {
+        KinBody::LinkConstPtr pLink = openravepy::GetKinBodyLinkConst(oincludedlinks[py::to_object(iLink)]);
+        if( !!pLink ) {
+            vIncludedLinks.push_back(pLink);
+        }
+        else {
+            RAVELOG_ERROR("failed to get included links\n");
+        }
+    }
 
-    bool bCollision = _probot->CheckLinkSelfCollision(ilinkindex, ExtractTransform(olinktrans), preport);
+    const TransformConstPtr pTransform = boost::make_shared<Transform>(ExtractTransform(olinktrans));
+    bool bCollision = _probot->CheckLinkSelfCollision(ilinkindex, vIncludedLinks, pTransform, preport);
     if( !!pyreport ) {
         pyreport->Init(report);
     }
@@ -2816,12 +2827,13 @@ void init_openravepy_robot()
 #ifdef USE_PYBIND11_PYTHON_BINDINGS
                        .def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision,
                             "linkindex"_a,
+                            "includedlinks"_a,
                             "linktrans"_a,
                             "report"_a = py::none_(), // PyCollisionReportPtr(),
                             DOXY_FN(RobotBase,CheckLinkSelfCollision)
                             )
 #else
-                       .def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, CheckLinkSelfCollision_overloads(PY_ARGS("linkindex", "linktrans", "report") DOXY_FN(RobotBase,CheckLinkSelfCollision)))
+                       .def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, CheckLinkSelfCollision_overloads(PY_ARGS("linkindex", "includedlinks", "linktrans", "report") DOXY_FN(RobotBase,CheckLinkSelfCollision)))
 #endif
                        .def("WaitForController",&PyRobotBase::WaitForController,PY_ARGS("timeout") "Wait until the robot controller is done")
                        .def("GetRobotStructureHash",&PyRobotBase::GetRobotStructureHash, DOXY_FN(RobotBase,GetRobotStructureHash))