From 4770d195072efea50dc9be19302e8ab08333f369 Mon Sep 17 00:00:00 2001 From: Sean Wilson Date: Wed, 20 Nov 2024 20:29:25 -0500 Subject: [PATCH 1/3] Added quick check to pass barrier calculation if only one robot is used. --- rps/utilities/barrier_certificates.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rps/utilities/barrier_certificates.py b/rps/utilities/barrier_certificates.py index da53c6f..2c28e90 100644 --- a/rps/utilities/barrier_certificates.py +++ b/rps/utilities/barrier_certificates.py @@ -55,6 +55,12 @@ def f(dxi, x): # Initialize some variables for computational savings N = dxi.shape[1] + + #If only one robot, no need for barriers. + if(N < 2) + return dxi + end + num_constraints = int(comb(N, 2)) A = np.zeros((num_constraints, 2*N)) b = np.zeros(num_constraints) From 2230d27e186924e936f4ae4ec33a651eece0783f Mon Sep 17 00:00:00 2001 From: Sean Date: Thu, 21 Nov 2024 21:38:17 -0500 Subject: [PATCH 2/3] Fixed silly mistake from editing MATLAB and Python simultaneously. --- rps/utilities/barrier_certificates.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/rps/utilities/barrier_certificates.py b/rps/utilities/barrier_certificates.py index 2c28e90..8b79c44 100644 --- a/rps/utilities/barrier_certificates.py +++ b/rps/utilities/barrier_certificates.py @@ -57,9 +57,8 @@ def f(dxi, x): N = dxi.shape[1] #If only one robot, no need for barriers. - if(N < 2) - return dxi - end + if(N < 2): + return dxi num_constraints = int(comb(N, 2)) A = np.zeros((num_constraints, 2*N)) @@ -508,10 +507,10 @@ def robust_barriers(dxu, x, obstacles=np.empty(0)): # Alternative Solver #start = time.time() - #vnew2 = solvers.qp(matrix(H), matrix(f), -matrix(A[0:count,0:2*num_robots]), -matrix( b[0:count]))['x'] # , A, b) Omit last 2 arguments since our QP has no equality constraints + vnew = qp(matrix(H), matrix(f), -matrix(A[0:count,0:2*num_robots]), -matrix( b[0:count]))['x'] # , A, b) Omit last 2 arguments since our QP has no equality constraints #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) - vnew = solver2.solve_qp(H, -np.squeeze(np.array(f)), A[0:count,0:2*num_robots].T, np.squeeze(np.array(b[0:count])))[0] + # vnew = solver2.solve_qp(H, -np.squeeze(np.array(f)), A[0:count,0:2*num_robots].T, np.squeeze(np.array(b[0:count])))[0] # Initial Guess for Solver at the Next Iteration # vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); # Set robot velocities to new velocities From 5bd07be424f1f17ca8f1c457dd740487d59bc780 Mon Sep 17 00:00:00 2001 From: Sean Date: Wed, 27 Nov 2024 23:53:32 -0500 Subject: [PATCH 3/3] Changed default avoidance radius to match MATLAB and be updated to new robot radius --- rps/utilities/barrier_certificates.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rps/utilities/barrier_certificates.py b/rps/utilities/barrier_certificates.py index 8b79c44..a28394e 100644 --- a/rps/utilities/barrier_certificates.py +++ b/rps/utilities/barrier_certificates.py @@ -37,7 +37,7 @@ def create_single_integrator_barrier_certificate(barrier_gain=100, safety_radius #Check user input ranges/sizes assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert safety_radius >= 0.12, "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m) plus the distance to the look ahead point used in the diffeomorphism if that is being used. Recieved %r." % safety_radius + assert safety_radius >= 0.15, "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m) plus the distance to the look ahead point used in the diffeomorphism if that is being used. Recieved %r." % safety_radius assert magnitude_limit > 0, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit assert magnitude_limit <= 0.2, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit