Allow to set security margin per body-body pair.

include/hpp/core/continuous-validation.hh

@@ -163,6 +163,11 @@ namespace hpp {
       /// \copydoc ObstacleUserInterface::setSecurityMargins
       virtual void setSecurityMargins(const matrix_t& securityMatrix);
 
+      /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
+      virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
+                                                  const std::string& body_b,
+                                                  const value_type& margin);
+
       /// \name Delegate
       /// \{
 

include/hpp/core/obstacle-user.hh

@@ -56,12 +56,24 @@ namespace hpp {
         ///
         /// \param relMotion square symmetric matrix of RelativeMotionType of size numberDof x numberDof
         virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion) = 0;
+
         /// Set different security margins for collision pairs
         ///
+        /// This function works joint-wise. If you need a finer control, use
+        /// \ref setSecurityMarginBetweenBodies
+        ///
         /// This method enables users to choose different security margins
-        /// for each pair of robot body or each pair robot body - obstacle.
+        /// for each pair of robot joint or each pair robot joint - obstacle.
         /// \sa hpp::fcl::CollisionRequest::security_margin.
         virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0;
+
+
+        /// Set security margin for collision pair between the two bodies.
+        ///
+        /// \sa hpp::fcl::CollisionRequest::security_margin.
+        virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
+                                                    const std::string& body_b,
+                                                    const value_type& margin) = 0;
     }; // class ObstacleUserInterface
 
     /// Vector of validation class instances.
@@ -129,11 +141,7 @@ namespace hpp {
           }
         }
 
-        /// Set different security margins for collision pairs
-        ///
-        /// This method enables users to choose different security margins
-        /// for each pair of robot body or each pair robot body - obstacle.
-        /// \sa hpp::fcl::CollisionRequest::security_margin.
+        /// \copydoc ObstacleUserInterface::setSecurityMargins
         void setSecurityMargins(const matrix_t& securityMatrix)
         {
           for (std::size_t i = 0; i < validations_.size(); ++i) {
@@ -143,6 +151,19 @@ namespace hpp {
           }
         }
 
+        /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
+        void setSecurityMarginBetweenBodies(const std::string& body_a,
+                                            const std::string& body_b,
+                                            const value_type& margin)
+        {
+          for (std::size_t i = 0; i < validations_.size(); ++i) {
+            shared_ptr<ObstacleUserInterface> oui =
+              HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
+            if (oui)
+              oui->setSecurityMarginBetweenBodies (body_a, body_b, margin);
+          }
+        }
+
         // Clear the vector of config validations
         void clear ()
         {
@@ -234,6 +255,12 @@ namespace hpp {
 
         /// \copydoc ObstacleUserInterface::setSecurityMargins
         virtual void setSecurityMargins(const matrix_t& securityMatrix);
+
+        /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
+        virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
+                                                    const std::string& body_b,
+                                                    const value_type& margin);
+
       protected:
         /// Constructor of body pair collision
         ObstacleUser (DevicePtr_t robot)

src/continuous-validation.cc

@@ -344,6 +344,37 @@ namespace hpp {
       bodyPairCollisionPool_.clear();
     }
 
+    void ContinuousValidation::setSecurityMarginBetweenBodies(
+        const std::string& body_a, const std::string& body_b,
+        const value_type& margin)
+    {
+      // Loop over collision pairs and remove disabled ones.
+      bool found = true;
+      for (IntervalValidations_t::iterator _colPair
+             (intervalValidations_.begin());
+          _colPair != intervalValidations_.end(); ++_colPair)
+      {
+        BodyPairCollisionPtr_t bpc(HPP_DYNAMIC_PTR_CAST(BodyPairCollision,
+                                                        *_colPair));
+        if (!bpc) continue;
+        const CollisionPairs_t& prs (bpc->pairs());
+        CollisionRequests_t& requests (bpc->requests());
+        for (std::size_t i = 0; i < prs.size(); ++i) {
+          const CollisionPair& pair (prs[i]);
+          if (  (pair.first->name() == body_a && pair.second->name() == body_b)
+              ||(pair.first->name() == body_b && pair.second->name() == body_a))
+          {
+            requests[i].security_margin = margin;
+            found = true;
+          }
+        }
+      }
+      if (!found)
+        throw std::invalid_argument("Could not find a collision pair between "
+            "body " + body_a + " and " + body_b);
+      bodyPairCollisionPool_.clear();
+    }
+
     template <>
     void ContinuousValidation::add<ContinuousValidation::AddObstacle>
     (const AddObstacle& delegate)

src/obstacle-user.cc

@@ -189,6 +189,29 @@ namespace hpp {
       }
     }
 
+    void ObstacleUser::setSecurityMarginBetweenBodies(const std::string& body_a,
+        const std::string& body_b, const value_type& margin)
+    {
+      auto setMargin = [&body_a, &body_b, &margin](const CollisionPairs_t& pairs,
+          CollisionRequests_t& requests) {
+        for (std::size_t i = 0; i < pairs.size(); ++i) {
+          const CollisionPair_t& pair = pairs[i];
+          if (  (pair.first->name() == body_a && pair.second->name() == body_b)
+              ||(pair.first->name() == body_b && pair.second->name() == body_a))
+          {
+            requests[i].security_margin = margin;
+            return true;
+          }
+        }
+        return false;
+      };
+      if (   setMargin(cPairs_, cRequests_)
+          && setMargin(pPairs_, pRequests_)
+          && setMargin(dPairs_, dRequests_)) return;
+      throw std::invalid_argument("Could not find a collision pair between body"
+          " " + body_a + " and " + body_b);
+    }
+
     void ObstacleUser::addRobotCollisionPairs ()
     {
       const pinocchio::GeomModel& model = robot_->geomModel();