diff --git a/include/colmpc/residual-distance-collision.hpp b/include/colmpc/residual-distance-collision.hpp
index bc044ff..d8d4a11 100644
--- a/include/colmpc/residual-distance-collision.hpp
+++ b/include/colmpc/residual-distance-collision.hpp
@@ -203,6 +203,9 @@ struct ResidualDataDistanceCollisionTpl
 
   pinocchio::SE3 oMg_id_1;
   pinocchio::SE3 oMg_id_2;
+
+  double dist_threshold = 0.08;  // compute derivatives only if collision
+                                 // distance is below threshold
 };
 
 }  // namespace colmpc
diff --git a/include/colmpc/residual-distance-collision.hxx b/include/colmpc/residual-distance-collision.hxx
index 28881c6..3789ed4 100644
--- a/include/colmpc/residual-distance-collision.hxx
+++ b/include/colmpc/residual-distance-collision.hxx
@@ -72,6 +72,10 @@ void ResidualDistanceCollisionTpl<Scalar>::calcDiff(
     const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
   Data *d = static_cast<Data *>(data.get());
 
+  if (d->r[0] > dist_threshold) {
+    d->Rx.fill(0);
+    return;
+  }
   const std::size_t nv = state_->get_nv();
 
   const auto &cp = geom_model_->collisionPairs[pair_id_];