Merge pull request #105 from mattrobotcontributor/resolve_collision_locking_force

kevinzakka · web-flow · commit 6a5d9195f74b · 2025-09-08T07:50:38.000-07:00
Resolve locking force when objects are already in collision.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -15,6 +15,7 @@ All notable changes to this project will be documented in this file.
 ### Bugfix
 
 - Fix `Configuration.get_inertia_matrix` for MuJoCo versions >= 3.3.4.
+- Objects already in collision repel each other instead of locking together.
 
 ## [0.0.11] - 2025-05-22
 
diff --git a/mink/limits/collision_avoidance_limit.py b/mink/limits/collision_avoidance_limit.py
@@ -206,7 +206,7 @@ def compute_qp_inequalities(
             jac = compute_contact_normal_jacobian(
                 self.model, configuration.data, contact
             )
-            coefficient_matrix[idx] = -jac
+            coefficient_matrix[idx] = -np.sign(hi_bound_dist) * jac
         return Constraint(G=coefficient_matrix, h=upper_bound)
 
     # Private methods.

Original file line number	Diff line number	Diff line change
`@@ -206,7 +206,7 @@ def compute_qp_inequalities(`
`206`	`206`	`jac = compute_contact_normal_jacobian(`
`207`	`207`	`self.model, configuration.data, contact`
`208`	`208`	`)`
`209`		`- coefficient_matrix[idx] = -jac`
	`209`	`+ coefficient_matrix[idx] = -np.sign(hi_bound_dist) * jac`
`210`	`210`	`return Constraint(G=coefficient_matrix, h=upper_bound)`
`211`	`211`
`212`	`212`	`# Private methods.`