diff --git a/rps/utilities/barrier_certificates.py b/rps/utilities/barrier_certificates.py index da53c6f..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 @@ -55,6 +55,11 @@ 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 + num_constraints = int(comb(N, 2)) A = np.zeros((num_constraints, 2*N)) b = np.zeros(num_constraints) @@ -502,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