diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index 0958119e1b753c815c44c241765c1c4efdac5246..9bfe494af70c100724c08ed9dcec515a1bb51de3 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -98,6 +98,11 @@ namespace hpp {
           static GraphPathValidationPtr_t create (
               const pinocchio::DevicePtr_t& robot, const value_type& stepSize);
 
+        /// Add obstacle in the environment
+        ///
+        /// Dynamic cast inner path validation into
+        /// hpp::core::ObstacleUserInterface and calls
+        /// hpp::core::ObstacleUserInterface::addObstacle in case of success.
         void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object)
         {
           boost::shared_ptr<core::ObstacleUserInterface> oui =
@@ -106,11 +111,11 @@ namespace hpp {
         }
 
         /// Remove a collision pair between a joint and an obstacle
-        /// \param joint the joint that holds the inner objects,
-        /// \param obstacle the obstacle to remove.
-        /// \notice collision configuration validation needs to know about
-        /// obstacles. This virtual method does nothing for configuration
-        /// validation methods that do not care about obstacles.
+        ///
+        /// Dynamic cast inner path validation into
+        /// hpp::core::ObstacleUserInterface and calls
+        /// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
+        /// of success.
         void removeObstacleFromJoint (const JointPtr_t& joint,
             const pinocchio::CollisionObjectConstPtr_t& obstacle)
         {
@@ -119,6 +124,12 @@ namespace hpp {
           if (oui) oui->removeObstacleFromJoint (joint, obstacle);
         }
 
+        /// \brief Filter collision pairs.
+        ///
+        /// Dynamic cast inner path validation into
+        /// hpp::core::ObstacleUserInterface and calls
+        /// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
+        /// success.
         void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion)
         {
           boost::shared_ptr<core::ObstacleUserInterface> oui =
@@ -126,6 +137,19 @@ namespace hpp {
           if (oui) oui->filterCollisionPairs (relMotion);
         }
 
+        /// Set different security margins for collision pairs
+        ///
+        /// Dynamic cast inner path validation into
+        /// hpp::core::ObstacleUserInterface and calls
+        /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
+        /// success.
+        void setSecurityMargins(const matrix_t& securityMargins)
+        {
+          boost::shared_ptr<core::ObstacleUserInterface> oui =
+            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+          if (oui) oui->setSecurityMargins(securityMargins);
+        }
+
       protected:
         /// Constructor
         GraphPathValidation (const PathValidationPtr_t& pathValidation);
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 864166fcc80653c381d9cafccda09d2823bc9e3c..3c9cdb3626b21fdf6674d501a86df9452a4d5a21 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -144,6 +144,16 @@ namespace hpp {
           /// Update the relative motion matrix
           void relativeMotion(const RelativeMotion::matrix_type & m);
 
+          /// Set Security margin for a pair of joints
+          ///
+          /// \param row index of joint1 + 1 in robot,
+          /// \param col index of joint2 + 1 in robot,
+          /// \param margin security margin for collision checking between
+          ///        those joints.
+          ///
+          /// \note set value to matrix [row, col] and matrix [col, row].
+          void securityMarginForPair(const size_type& row, const size_type& col,
+                                     const value_type& margin);
           /// Get direction of the path compare to the edge
           /// \return true is reverse
           virtual bool direction (const core::PathPtr_t& path) const;
@@ -208,6 +218,9 @@ namespace hpp {
 
 	  /// Path validation associated to the edge
           mutable RelativeMotion::matrix_type relMotion_;
+          /// matrix of security margins for collision checking between joints
+          matrix_t securityMargins_;
+
           core::PathValidationPtr_t pathValidation_;
 
           /// Weak pointer to itself.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 0d7b21aa8df3292ae94118240af8ef0f43cc9441..f08aa4bc8442e088fb8cd3f7ef285631bf65798b 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -43,6 +43,7 @@ namespace hpp {
         pathConstraints_ (),
 	configConstraints_ (),
         steeringMethod_ (),
+        securityMargins_ (),
         pathValidation_ ()
       {}
 
@@ -106,6 +107,28 @@ namespace hpp {
         return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
       }
 
+      void Edge::securityMarginForPair
+      (const size_type& row, const size_type& col, const value_type& margin)
+      {
+        if ((row < 0) || (row >= securityMargins_.rows())) {
+          std::ostringstream os;
+          os << "Row index should be between 0 and "
+             << securityMargins_.rows()+1 << ", got " << row << ".";
+          throw std::runtime_error(os.str().c_str());
+        }
+        if ((col < 0) || (col >= securityMargins_.cols())) {
+          std::ostringstream os;
+          os << "Column index should be between 0 and "
+             << securityMargins_.cols()+1 << ", got " << col << ".";
+          throw std::runtime_error(os.str().c_str());
+        }
+        if (securityMargins_(row, col) != margin) {
+          securityMargins_(row, col) = margin;
+          securityMargins_(col, row) = margin;
+          isInit_ = false;
+        }
+      }
+
       bool Edge::intersectionConstraint (const EdgePtr_t& other,
           ConfigProjectorPtr_t proj) const
       {
@@ -144,6 +167,10 @@ namespace hpp {
         from_ = from;
         to_ = to;
         state_ = to;
+        // add 1 joint for the environment
+        size_type n (graph.lock()->robot()->nbJoints() + 1);
+        securityMargins_.resize(n, n);
+        securityMargins_.setZero();
       }
 
       void Edge::initialize ()
@@ -305,6 +332,7 @@ namespace hpp {
           relMotion_ = RelativeMotion::matrix (g->robot());
           RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
           oui->filterCollisionPairs (relMotion_);
+          oui->setSecurityMargins(securityMargins_);
         }
         return constraint;
       }