diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 856ed28..29aa624 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -85,13 +85,13 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b if not fatrop_solver: # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj total_ek = ocp.sum(ek,grid='control',include_last=True) - ocp.subject_to(total_ek < N*rms_error_traj**2) + ocp.subject_to(total_ek < (N*rms_error_traj**2)) else: # Fatrop does not support summing over grid points inside a constraint, so we implement the sum using a running cost to achieve the same result as above running_ek = ocp.state() # running sum of squared error ocp.subject_to(ocp.at_t0(running_ek == 0)) ocp.set_next(running_ek, running_ek + ek) # sum over the control grid - ocp.subject_to(ocp.at_tf( (running_ek + ek)/(N*rms_error_traj**2) < 1 )) + ocp.subject_to(ocp.at_tf( (running_ek + ek) < (N*rms_error_traj**2) )) # TODO this is still needed because last sample is not included in the sum now #total_ek = ocp.state() # total sum of squared error