consistency optistack and rockit

trajectory-invariants · Sep 27, 2024 · e5b9fe1 · e5b9fe1
1 parent 9b46b44
commit e5b9fe1
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py
@@ -63,7 +63,7 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_error
         for k in range(window_len):
             err_pos = p_obj[k] - p_obj_m[k] # position error
             trajectory_error = trajectory_error + cas.dot(err_pos,err_pos)    
-        opti.subject_to(trajectory_error/window_len < rms_error_traj**2)
+        opti.subject_to(trajectory_error  < window_len*rms_error_traj**2)
 
         # Boundary constraints
         #opti.subject_to(self.p_obj[0] == self.p_obj_m[0]) # Fix first measurement