From ba370bf860e2b5c30319a37f886257bccd26b8c3 Mon Sep 17 00:00:00 2001
From: Dongya Jiang <jiangdongya@xiaoyubot.com>
Date: Fri, 22 Dec 2023 11:42:41 +0800
Subject: [PATCH] Fix penalty calculation in constraints cost function in STOMP

Signed-off-by: Dongya Jiang <jiangdongya@xiaoyubot.com>
---
 moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp
index 4d1dd2993a..488510f15d 100644
--- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp
+++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp
@@ -212,7 +212,8 @@ CostFn getConstraintsCostFunction(const std::shared_ptr<const planning_scene::Pl
     setJointPositions(positions, joints, state);
     state.update();
 
-    return constraints.decide(state).distance * cost_scale;
+    const auto& result = constraints.decide(state);
+    return result.satisfied ? 0.0 : result.distance * cost_scale;
   };
 
   return getCostFunctionFromStateValidator(constraints_validator_fn, CONSTRAINT_CHECK_DISTANCE);