From e2794f469e0aa3eafd653214977d5411dc3d8e8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Tue, 8 Jul 2025 08:54:38 +0000 Subject: [PATCH 1/2] add threshold to improve efficiency --- include/colmpc/residual-distance-collision.hpp | 2 ++ include/colmpc/residual-distance-collision.hxx | 6 +++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/include/colmpc/residual-distance-collision.hpp b/include/colmpc/residual-distance-collision.hpp index bc044ff..3b9aad8 100644 --- a/include/colmpc/residual-distance-collision.hpp +++ b/include/colmpc/residual-distance-collision.hpp @@ -203,6 +203,8 @@ 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..14cc425 100644 --- a/include/colmpc/residual-distance-collision.hxx +++ b/include/colmpc/residual-distance-collision.hxx @@ -71,7 +71,11 @@ void ResidualDistanceCollisionTpl::calcDiff( const std::shared_ptr &data, const Eigen::Ref &x, const Eigen::Ref &) { Data *d = static_cast(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_]; From 23ba1c80ad69db1ce200a11b5aa8f92def29ad57 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Jul 2025 08:57:36 +0000 Subject: [PATCH 2/2] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/colmpc/residual-distance-collision.hpp | 3 ++- include/colmpc/residual-distance-collision.hxx | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/colmpc/residual-distance-collision.hpp b/include/colmpc/residual-distance-collision.hpp index 3b9aad8..d8d4a11 100644 --- a/include/colmpc/residual-distance-collision.hpp +++ b/include/colmpc/residual-distance-collision.hpp @@ -204,7 +204,8 @@ 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 + 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 14cc425..3789ed4 100644 --- a/include/colmpc/residual-distance-collision.hxx +++ b/include/colmpc/residual-distance-collision.hxx @@ -71,8 +71,8 @@ void ResidualDistanceCollisionTpl::calcDiff( const std::shared_ptr &data, const Eigen::Ref &x, const Eigen::Ref &) { Data *d = static_cast(data.get()); - - if (d->r[0] > dist_threshold){ + + if (d->r[0] > dist_threshold) { d->Rx.fill(0); return; }