├── .gitignore ├── LICENSE ├── README.md ├── clf_cbf_nmpc.py ├── estimator.py ├── gifs ├── demo.gif ├── demo1.gif ├── demo2.gif └── demo3.gif ├── observer.py ├── rps ├── __init__.py ├── __version__.py ├── examples │ ├── __init__.py │ ├── barrier_certificates │ │ ├── barrier_certificates.py │ │ ├── si_barriers_with_boundary.py │ │ ├── uni_barriers_with_boundary.py │ │ └── uni_dd_barriers_with_boundary.py │ ├── consensus │ │ ├── consensus.py │ │ └── consensus_fewer_errors.py │ ├── data_saving │ │ └── leader_follower_save_data.py │ ├── formation_control │ │ └── formation_control.py │ ├── go_to_point │ │ ├── si_go_to_point.py │ │ └── uni_go_to_point.py │ ├── go_to_pose │ │ ├── uni_go_to_pose_clf.py │ │ └── uni_go_to_pose_hybrid.py │ ├── leader_follower_static │ │ └── leader_follower.py │ ├── plotting │ │ ├── GTLogo.png │ │ ├── barrier_certificates_with_plotting.py │ │ ├── leader_follower_with_plotting.py │ │ ├── si_go_to_point_gt.py │ │ ├── si_go_to_point_with_plotting.py │ │ └── uni_go_to_pose_hybrid_with_plotting.py │ └── ra │ │ └── ra.py ├── robotarium.py ├── robotarium.py.bak ├── robotarium_abc.py ├── robotarium_abc.py.bak └── utilities │ ├── __init__.py │ ├── barrier_certificates.py │ ├── barrier_certificates2.py │ ├── controllers.py │ ├── graph.py │ ├── misc.py │ └── transformations.py └── test.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_Store 2 | *.pyc 3 | 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Anxing Xiao, Hao Luan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CLF_CBF_NMPC_python 2 | 3 | An unofficial python implement of nonlinear model predictive control with control Lyapunov functions and control barrier functions (CLF-CBF-NMPC). 4 | (Details are in [https://arxiv.org/abs/2105.10596](https://arxiv.org/abs/2105.10596)) 5 | 6 | ## Simulation Environment 7 | The test code `test.py` is based on the simulation environment [Robotarium (https://github.com/robotarium/robotarium_python_simulator)](https://github.com/robotarium/robotarium_python_simulator). 8 | Directory `\rps` contains a modified *Robotarium* environment specifically tuned for a reach-avoid task in a crowded environment. 9 | 10 | 11 | ## Demonstration 12 | 13 | - The yellow dubin car's objective is to reach a goal area (the rectangle) while avoiding all other blue agents running in the scene. 14 | - This CLF-CBF-NMPC module provides a motion planning solution. 15 | 16 | ![avatar](./gifs/demo.gif) 17 | 18 | ![avatar](./gifs/demo1.gif) 19 | 20 | ![avatar](./gifs/demo2.gif) 21 | 22 | ![avatar](./gifs/demo3.gif) 23 | 24 | -------------------------------------------------------------------------------- /clf_cbf_nmpc.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2021 Hao Luan, Anxing Xiao 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import numpy as np 25 | import casadi as ca 26 | 27 | class CLF_CBF_NMPC(): 28 | """ 29 | A class encapsulating the CLF-CBF-NMPC method, implemented using CasADi. 30 | 31 | Detailed description. 32 | 33 | Attributes: 34 | N : MPC horizon 35 | M_CBF : CBF constraints horizon 36 | M_CLF : CLF constraints horizon 37 | gamma_k : (1-\\gamma_k) is the exponential decreasing rate of a CBF at time step k 38 | alpha_k : (1-\\alpha_k) is the exponential decreasing rate of a CLF at time step k 39 | step : discretization time step in seconds 40 | goal_global : goal state in global coordinates 41 | 42 | opti : CasADi's Opti stack 43 | opt_x0_global : initial state in global coordinates 44 | opt_x0_local : initial state in global coordinates 45 | opt_states : optimized states/trajectory 46 | opt_controls : optimized control variables 47 | opt_trj : optimized trajectory (in global coordinates) 48 | opt_d_slack : a slack variable resolving infeasibility between CLF and CBF 49 | opt_cost : 50 | 51 | 52 | goal_local : goal state in local coordinates 53 | v_max : Maximum linear velocity 54 | omega_max : Maximum angular velocity 55 | """ 56 | 57 | ## parameters for optimization 58 | step = 0.1 59 | N = 12 60 | M_CBF = 6 61 | M_CLF = 1 62 | gamma_k = 0.1 63 | alpha_k = 0.01 64 | opti = ca.Opti() 65 | 66 | opt_x0_global = opti.parameter() 67 | opt_x0_local = opti.parameter() 68 | opt_controls = opti.variable(N) 69 | opt_states = opti.variable(N+1) 70 | opt_d_slack = opti.variable(N) 71 | opt_cost = 0 72 | 73 | v_max = 0.5 74 | omega_max = 1.5 75 | 76 | def __init__(self, n, stp, m_cbf, m_clf, gamma, alpha, dim_x = 3, dim_u = 2): 77 | self.N = n 78 | self.step = stp 79 | self.M_CBF = m_cbf 80 | self.M_CLF = m_clf 81 | self.gamma_k = gamma 82 | self.alpha_k = alpha 83 | self.opti = ca.Opti() 84 | 85 | self.v_max = 0.45 86 | self.omega_max = 1.5 87 | # self.v_max = v_m 88 | # self.omega_max = omg_m 89 | 90 | # state variables 91 | self.opt_states = self.opti.variable(self.N+1, dim_x) 92 | # control variables: linear velocity v and angle velocity omega 93 | self.opt_controls = self.opti.variable(self.N, dim_u) 94 | # a slack variable to avoid infeasibility between CLF and CBF constraints 95 | self.opt_d_slack = self.opti.variable(self.N, 1) 96 | 97 | # initial state 98 | self.opt_x0_local = self.opti.parameter(dim_x) 99 | self.opt_x0_global = self.opti.parameter(dim_x) 100 | self.goal_global = self.opti.parameter(dim_x) 101 | self.goal_local = self.opti.parameter(dim_x) 102 | 103 | # set up cost function 104 | self.__set_cost_func() 105 | 106 | 107 | def reset(self): 108 | # clear all constraints 109 | self.opti.subject_to() 110 | self.opt_cost = 0 111 | self.__set_cost_func() 112 | 113 | def set_goal(self, goal_state): 114 | """ 115 | Set goal states (in globla coordinates) 116 | 117 | Turn a vector into a matrix. 118 | 119 | Args: 120 | goal_global: : vector/list 121 | 122 | Returns: 123 | None. 124 | """ 125 | self.opti.set_value(self.goal_global, goal_state) 126 | self.opti.set_value(self.goal_local, goal_state) 127 | 128 | def __set_cost_func(self): 129 | # quadratic cost : J = (x-goal)^T * Q * (x-goal) + u^T * R * u + W_slack * opt_d_slack 130 | # Q : Weights for state variables in a (quadratic) cost function 131 | # R : Weights for control input variables in a (quadratic) cost function 132 | Q = np.array([[1, 0.0, 0.0],[0.0, 1, 0.0],[0.0, 0.0, 0.0]]) 133 | R = np.array([[0.01, 0.0], [0.0, 0.00001]]) 134 | W_slack = np.array([[1000]]) 135 | for i in range(self.N): 136 | self.opt_cost = self.opt_cost + ca.mtimes([self.opt_controls[i, :], R, self.opt_controls[i, :].T]) + ca.mtimes([self.opt_d_slack[i, :], W_slack, self.opt_d_slack[i, :].T]) 137 | # self.opt_cost = self.opt_cost + ca.mtimes([(self.opt_states[i, :] - self.goal_local.T), Q, (self.opt_states[i, :]- self.goal_local.T).T]) / 5 138 | # final term 139 | self.opt_cost = self.opt_cost + ca.mtimes([(self.opt_states[self.N-1, :] - self.goal_local.T), Q, (self.opt_states[self.N-1, :]- self.goal_local.T).T]) 140 | 141 | 142 | def __global2local(self): 143 | """ 144 | Transform global coordinates into local coordinates w.r.t. the robot 145 | 146 | Robot is always at the origin (0,0, theta). Orientation is left as is. 147 | 148 | Args: 149 | None. 150 | Returns: 151 | None. 152 | """ 153 | init_st = self.opti.value(self.opt_x0_global) 154 | # self.opti.set_value(self.goal_local, self.opti.value(self.goal_global) - init_st) 155 | # self.opti.set_value(self.opt_x0_local, self.opti.value(self.opt_x0_global) - init_st) 156 | self.goal_local[0] = self.goal_global[0] -init_st[0] 157 | self.goal_local[1] = self.goal_global[1] -init_st[1] 158 | self.opt_x0_local[0] = self.opt_x0_global[0] -init_st[0] 159 | self.opt_x0_local[1] = self.opt_x0_global[1] -init_st[1] 160 | # time k 161 | for t_k in self.obstacles: 162 | # obstacle i at time k 163 | for obs_i in t_k: 164 | obs_i[0] -= init_st[0] 165 | obs_i[1] -= init_st[1] 166 | 167 | 168 | # print('Goal_global:') 169 | # print(self.opti.value(self.goal_global)) 170 | # # print('Goal_local:') 171 | # print(self.opti.value(self.goal_local)) 172 | 173 | def __local2global(self): 174 | delta = self.opti.value(self.opt_x0_global-self.opt_x0_local) 175 | self.opt_trj = self.solution.value(self.opt_states).copy() 176 | # state at time k 177 | for st_k in self.opt_trj: 178 | # only transform back x and y coordinates 179 | self.opt_trj[0] += delta[0] 180 | self.opt_trj[1] += delta[1] 181 | 182 | def __add_system_constrnts(self): 183 | x = self.opt_states[:, 0] 184 | y = self.opt_states[:, 1] 185 | # theta = self.opt_states[:, 2] 186 | v = self.opt_controls[:, 0] 187 | omega = self.opt_controls[:, 1] 188 | # position boundaries (not necessary) 189 | delta = self.opti.value(self.opt_x0_global-self.opt_x0_local) 190 | self.opti.subject_to(self.opti.bounded(-1.45, x+delta[0], 1.45)) 191 | self.opti.subject_to(self.opti.bounded(-1.45, y+delta[1], 1.45)) 192 | # admissable control constraints 193 | self.opti.subject_to(self.opti.bounded(-self.v_max, v, self.v_max)) 194 | self.opti.subject_to(self.opti.bounded(-self.omega_max, omega, self.omega_max)) 195 | 196 | def __add_dynamics_constrnts(self): 197 | # create system dynamics x(t+1) = x(t) + f(x(t), u) * step 198 | f = lambda x_, u_: ca.vertcat(*[u_[0]*ca.cos(x_[2]), u_[0]*ca.sin(x_[2]), u_[1]]) 199 | 200 | # initial condition constraint 201 | self.opti.subject_to(self.opt_states[0, :] == self.opt_x0_local.T) # In local, x0 == [vec(0), theta] 202 | # self.opti.subject_to(self.opt_states[0, :] == self.opt_x0.T) 203 | 204 | # Dynamics constraints 205 | for i in range(self.N): 206 | x_next = self.opt_states[i, :] + self.step*f(self.opt_states[i, :], self.opt_controls[i, :]).T 207 | self.opti.subject_to(self.opt_states[i+1, :]==x_next) 208 | 209 | def __add_safe_constrnts(self): 210 | # safe distance 211 | safe_dist = 0.24 212 | # control barrier function 213 | h = lambda x_,y_: (x_[0] - y_[0]) ** 2 + (x_[1] - y_[1]) ** 2 - safe_dist**2 214 | 215 | # add CBF constraints 216 | for i in range(self.M_CBF): 217 | # current and future safety contraints 218 | # others_states[i][j]: agent No. j+1 's state at i steps ahead, these are provided by an observer & a predictor 219 | for j in range(np.size(self.obstacles, 1)): 220 | self.opti.subject_to(h(self.opt_states[i+1, :], self.obstacles[i+1][j]) >= (1-self.gamma_k)*h(self.opt_states[i, :], self.obstacles[i][j]) ) 221 | 222 | def __add_stability_constrnts(self): 223 | # control Lyapunov function 224 | V = lambda x_: (x_[0] - self.goal_local[0]) ** 2 + (x_[1] - self.goal_local[1]) ** 2 225 | # add CLF constraints 226 | for i in range(self.M_CLF): 227 | self.opti.subject_to(V(self.opt_states[i+1, :]) <= (1-self.alpha_k)*V(self.opt_states[i, :]) + self.opt_d_slack[i, :]) 228 | pass 229 | 230 | def __adopt_d_slack_constrnts(self): 231 | pass 232 | 233 | def solve(self, init_st, goal_st, others_states): 234 | """ 235 | Solving the CLF-CBF-NMPC optimization. 236 | 237 | Initializing the solver is required before first-time using. 238 | 239 | 240 | Args: 241 | init_st : initial state of the robot 242 | others_states : all other agents' (or obstacles') states at CURRENT step AND some FUTURE steps 243 | 244 | Returns: 245 | global_states_sol : optimized trajectory (in global coordinates) 246 | controls_sol : optimized control inputs trajectory 247 | d_slck_sol : optimized slack variable 248 | local_states_sol : optimized trajectory (in local coordinates) 249 | """ 250 | # Reset the optimizer FIRST! 251 | self.reset() 252 | # print("Optimizer reset.") 253 | 254 | # set initial states 255 | self.opti.set_value(self.opt_x0_global, init_st) 256 | self.opti.set_value(self.opt_x0_local, init_st) 257 | # set goal states 258 | self.set_goal(goal_st) 259 | 260 | # obstacles or other agents 261 | self.obstacles = others_states.copy() 262 | # only care about obstacles within an ROI 263 | l = [] 264 | for i in range(self.obstacles.shape[1]): 265 | if((self.obstacles[0][i][0] - init_st[0])** 2 + (self.obstacles[0][i][1] - init_st[1])** 2 > 1): 266 | l.append(i) 267 | # delete those out of the ROI 268 | self.obstacles = np.delete(self.obstacles, l, axis=1) 269 | # print(self.obstacles.shape) 270 | 271 | 272 | # Turn everything into local coordinates. 273 | self.__global2local() # Very IMPORTANT!! 274 | # print('Coordinates transformed to local.') 275 | 276 | # Adding constraints to the optimizer 277 | self.__add_system_constrnts() 278 | self.__add_dynamics_constrnts() 279 | self.__add_safe_constrnts() 280 | self.__add_stability_constrnts() 281 | 282 | # Optimizer configures 283 | self.opti.minimize(self.opt_cost) 284 | opts_setting = {'ipopt.max_iter':200, 'ipopt.print_level':1, 'print_time':0, 'ipopt.acceptable_tol':1e-5, 'ipopt.acceptable_obj_change_tol':1e-5} 285 | self.opti.solver('ipopt', opts_setting) 286 | # self.opti.solver('ipopt') 287 | # self.solution = self.opti.solve() 288 | 289 | 290 | # Solve 291 | try: 292 | self.solution = self.opti.solve() 293 | local_states_sol = self.opti.value(self.opt_states) 294 | controls_sol = self.opti.value(self.opt_controls) 295 | # d_slck_sol = self.opti.value(self.opt_d_slack) 296 | self.__local2global() 297 | global_states_sol = self.opt_trj 298 | except: 299 | print('Something went wrong!!') 300 | local_states_sol = self.opti.debug.value(self.opt_states) 301 | controls_sol = self.opti.debug.value(self.opt_controls) 302 | # d_slck_sol = self.opti.debug.value(self.opt_d_slack) 303 | self.__local2global() 304 | global_states_sol = self.opt_trj 305 | 306 | return global_states_sol, controls_sol, local_states_sol #, d_slck_sol, 307 | # return self.solution 308 | 309 | def get_optimized_u(self): 310 | print('Optimized controls:') 311 | print(self.solution.value(self.opt_controls)[0, :]) 312 | return self.solution.value(self.opt_controls)[0, :] 313 | 314 | 315 | 316 | def Simple_Catcher(attacker_state,defender_state): 317 | is_poistive = 1 318 | distance = np.sqrt(np.sum(np.square(attacker_state[:2] - defender_state[:2]))) 319 | dx =attacker_state[0] - defender_state[0] 320 | dy =attacker_state[1] - defender_state[1] 321 | theta_e = np.arctan2(dy,dx) - defender_state[2] 322 | # print(np.arctan(dy/dx)) 323 | # print(defender_state[2]) 324 | # attacker_state[2] - defender_state[2] 325 | if(theta_e>np.pi): 326 | theta_e -= 2*np.pi 327 | elif (theta_e<-np.pi): 328 | theta_e += 2*np.pi 329 | # print(theta_e) 330 | 331 | if(theta_e>np.pi/2 or theta_e<-np.pi/2): 332 | is_poistive = -1 333 | 334 | u = 1.5*distance*is_poistive 335 | w = theta_e*is_poistive 336 | u = np.clip(u, -0.32, 0.32) 337 | w = np.clip(w, -2, 2) 338 | return np.array([u,w]) 339 | 340 | 341 | ## Test Code 342 | if __name__ == '__main__': 343 | states = np.array([[-1,0,0],[0.5,0,-np.pi]]) 344 | u = CLF_CBF_NMPC(states[0],states[1]) 345 | print(u) 346 | -------------------------------------------------------------------------------- /estimator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Estimator: 4 | """ 5 | A simple estimator that predicts systems' future states 6 | 7 | Given systems' dynamics, inital states, time step, and a prediciton horizon, this estimator returns all future states of the agent governed by given dynamics within the prediction horizon. 8 | 9 | Attributes: 10 | cur_state : current state 11 | ctrl_input : control input u of dynamics f(x, u) 12 | step : discretization time step 13 | dynamics : system evolving dynamics f(x, u) 14 | window : prediciton horizon 15 | predict_states : predicting results 16 | """ 17 | cur_state = np.array([0]) 18 | ctrl_input = np.array([0]) 19 | step = 0.1 20 | dynamics = lambda x_, v_: x_ + step * v_ 21 | window = 1 22 | predict_states = np.zeros(window) 23 | 24 | def __init__(self, x0, u, dyn, stp, windw): 25 | self.cur_state = x0 26 | self.ctrl_input = u 27 | self.step = stp 28 | self.dynamics = dyn 29 | self.window = windw 30 | self.predict_states = np.zeros(windw) 31 | 32 | def predict(self): 33 | x0 = self.cur_state 34 | dyn = self.dynamics 35 | u = self.ctrl_input 36 | 37 | # System evolves: x(t+1) = x(t) + T * f(x(t), u) 38 | x_nxt = x0 + self.step * dyn(x0, u) 39 | self.predict_states = np.array([x_nxt]) 40 | x0 = x_nxt 41 | for t in range(1, self.window): 42 | x_nxt = x0 + self.step * dyn(x0, u) 43 | self.predict_states = np.concatenate((self.predict_states, np.array([x_nxt])), axis=0) 44 | x0 = x_nxt 45 | 46 | 47 | 48 | ## Test code 49 | if __name__ == '__main__': 50 | # Unicycle model 51 | dyn = lambda x_, u_: np.array([u_.T[0]*np.cos(x_.T[2]), u_.T[0]*np.sin(x_.T[2]), u_.T[1]]).T 52 | est = Estimator(np.array([[0, 0, 0], [1,2,3], [4,5,6]]), np.array([[1, 0], [-1, 0], [0, 0.1]]), dyn, 0.1, 10) 53 | est.predict() 54 | print(est.predict_states) 55 | 56 | 57 | -------------------------------------------------------------------------------- /gifs/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/gifs/demo.gif -------------------------------------------------------------------------------- /gifs/demo1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/gifs/demo1.gif -------------------------------------------------------------------------------- /gifs/demo2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/gifs/demo2.gif -------------------------------------------------------------------------------- /gifs/demo3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/gifs/demo3.gif -------------------------------------------------------------------------------- /observer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | class Observer: 5 | """ 6 | A simple observer that assumes targets move in straight lines at a constant speed. 7 | 8 | Take in full observation, return estimated full states and estimated velocities. 9 | 10 | Attributes: 11 | observation_history : emmmmm... 12 | vel : velocities 13 | window : history length, i.e. history for how long before now we would like to keep 14 | step : discretization time step 15 | r_roi : Radius of region of interest 16 | """ 17 | window = 5 18 | observation_history = [0]*window 19 | step = 0.1 20 | vel = 0 21 | states = 0 22 | r_roi = 1 23 | 24 | 25 | def __init__(self, x0, stp, windw, r=1): 26 | self.window = windw 27 | self.observation_history = [x0]*windw 28 | self.step = stp 29 | self.states = x0 30 | self.r_roi = r 31 | 32 | 33 | 34 | def feed(self, new_obsrv): 35 | # print('1',self.observation_history) 36 | obsrv = new_obsrv.copy() 37 | # print(obsrv) 38 | self.observation_history.pop(0) # Discard the oldest observation in the slot 39 | self.observation_history.append(obsrv) 40 | 41 | # Successive difference method calculating velocities 42 | num_grp = math.floor(self.window/2) 43 | sum = self.observation_history[self.window-1] - self.observation_history[self.window-1] 44 | for i in range(1, num_grp+1): 45 | sum = sum + self.observation_history[self.window-i] - self.observation_history[self.window-num_grp-i] 46 | self.states = new_obsrv 47 | self.vel = sum / num_grp / (self.step*num_grp) 48 | 49 | 50 | 51 | ## Test 52 | if __name__ == '__main__': 53 | v = 1 54 | T = 0.01 55 | x = np.array([[0, 0], [1, 1], [2,2]]) 56 | observer = Observer(x, T, 5) 57 | for k in range(1, 10): 58 | x = x + v*T 59 | observer.feed(x) 60 | print(k) 61 | print(observer.observation_history) 62 | print(observer.vel) 63 | -------------------------------------------------------------------------------- /rps/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/rps/__init__.py -------------------------------------------------------------------------------- /rps/__version__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/rps/__version__.py -------------------------------------------------------------------------------- /rps/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/rps/examples/__init__.py -------------------------------------------------------------------------------- /rps/examples/barrier_certificates/barrier_certificates.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | 9 | # How many robots we want to use in the simulation 10 | N = 10 11 | # Instantiate the Robotarium object with these parameters 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 13 | 14 | # How many times should the robots form the circle? 15 | num_cycles=2 16 | count = -1 # How many times have they formed the circle? (starts at -1 since initial formation will increment the count) 17 | 18 | # We're working in single-integrator dynamics, and we don't want the robots 19 | # to collide. Thus, we're going to use barrier certificates 20 | si_barrier_cert = create_single_integrator_barrier_certificate() 21 | 22 | # Create single integrator position controller 23 | si_position_controller = create_si_position_controller() 24 | 25 | # Create SI to UNI dynamics tranformation 26 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 27 | 28 | # This portion of the code generates points on a circle enscribed in a 6x6 square 29 | # that's centered on the origin. The robots switch positions on the circle. 30 | radius = 1 31 | xybound = radius*np.array([-1, 1, -1, 1]) 32 | p_theta = 2*np.pi*(np.arange(0, 2*N, 2)/(2*N)) 33 | p_circ = np.vstack([ 34 | np.hstack([xybound[1]*np.cos(p_theta), xybound[1]*np.cos(p_theta+np.pi)]), 35 | np.hstack([xybound[3]*np.sin(p_theta), xybound[3]*np.sin(p_theta+np.pi)]) 36 | ]) 37 | 38 | # These variables are so we can tell when the robots should switch positions 39 | # on the circle. 40 | flag = 0 41 | x_goal = p_circ[:, :N] 42 | 43 | # Perform the simulation for a certain number of iterations 44 | while(1): 45 | 46 | # Get the poses of the agents that we want 47 | x = r.get_poses() 48 | 49 | # To compare distances, only take the first two elements of our pose array. 50 | x_si = uni_to_si_states(x) 51 | 52 | # Initialize a velocities variable 53 | si_velocities = np.zeros((2, N)) 54 | 55 | # Check if all the agents are close enough to the goals 56 | if(np.linalg.norm(x_goal - x_si) < 0.05): 57 | flag = 1-flag 58 | count += 1 59 | 60 | if count == num_cycles: 61 | break 62 | 63 | # Switch goals depending on the state of flag (goals switch to opposite 64 | # sides of the circle) 65 | if(flag == 0): 66 | x_goal = p_circ[:, :N] 67 | else: 68 | x_goal = p_circ[:, N:] 69 | 70 | # Use a position controller to drive to the goal position 71 | dxi = si_position_controller(x_si,x_goal) 72 | 73 | # Use the barrier certificates to make sure that the agents don't collide 74 | dxi = si_barrier_cert(dxi, x_si) 75 | 76 | # Use the second single-integrator-to-unicycle mapping to map to unicycle 77 | # dynamics 78 | dxu = si_to_uni_dyn(dxi, x) 79 | 80 | # Set the velocities of agents 1,...,N to dxu 81 | r.set_velocities(np.arange(N), dxu) 82 | # Iterate the simulation 83 | r.step() 84 | 85 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 86 | r.call_at_scripts_end() 87 | -------------------------------------------------------------------------------- /rps/examples/barrier_certificates/si_barriers_with_boundary.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | 9 | # Instantiate Robotarium object 10 | N = 5 11 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 13 | 14 | # The robots will never reach their goal points so set iteration number 15 | iterations = 3000 16 | 17 | # Define goal points outside of the arena 18 | goal_points = np.array(np.mat('5 -5 5 -5 5; 5 5 -5 -5 5; 0 0 0 0 0')) 19 | 20 | # Create unicycle position controller 21 | si_position_controller = create_si_position_controller() 22 | 23 | # Create SI to UNI dynamics tranformation 24 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 25 | 26 | # Create barrier certificates to avoid collision 27 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 28 | 29 | 30 | # define x initially 31 | x = r.get_poses() 32 | r.step() 33 | 34 | # While the number of robots at the required poses is less 35 | # than N... 36 | for i in range(iterations): 37 | 38 | # Get poses of agents 39 | x = r.get_poses() 40 | xi = uni_to_si_states(x) 41 | 42 | # Create single-integrator control inputs 43 | dxi = si_position_controller(xi, goal_points[:2, :]) 44 | 45 | # Create safe control inputs (i.e., no collisions) 46 | dxi = si_barrier_cert(dxi, xi) 47 | 48 | # Map the single integrator back to unicycle dynamics 49 | dxu = si_to_uni_dyn(dxi,x) 50 | 51 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 52 | r.set_velocities(np.arange(N), dxu) 53 | 54 | # Iterate the simulation 55 | r.step() 56 | 57 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 58 | r.call_at_scripts_end() 59 | -------------------------------------------------------------------------------- /rps/examples/barrier_certificates/uni_barriers_with_boundary.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | 9 | # Instantiate Robotarium object 10 | N = 5 11 | 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 13 | 14 | # The robots will never reach their goal points so set iteration number 15 | iterations = 3000 16 | 17 | # Define goal points outside of the arena 18 | goal_points = np.array(np.mat('5 5 5 5 5; 5 5 5 5 5; 0 0 0 0 0')) 19 | 20 | # Create unicycle position controller 21 | unicycle_position_controller = create_clf_unicycle_position_controller() 22 | 23 | # Create barrier certificates to avoid collision 24 | uni_barrier_cert = create_unicycle_barrier_certificate_with_boundary() 25 | 26 | 27 | # define x initially 28 | x = r.get_poses() 29 | r.step() 30 | 31 | # While the number of robots at the required poses is less 32 | # than N... 33 | for i in range(iterations): 34 | 35 | # Get poses of agents 36 | x = r.get_poses() 37 | 38 | # Create single-integrator control inputs 39 | dxu = unicycle_position_controller(x, goal_points[:2][:]) 40 | 41 | # Create safe control inputs (i.e., no collisions) 42 | dxu = uni_barrier_cert(dxu, x) 43 | 44 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 45 | r.set_velocities(np.arange(N), dxu) 46 | 47 | # Iterate the simulation 48 | r.step() 49 | 50 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 51 | r.call_at_scripts_end() 52 | -------------------------------------------------------------------------------- /rps/examples/barrier_certificates/uni_dd_barriers_with_boundary.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | 9 | # Instantiate Robotarium object 10 | N = 5 11 | 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 13 | 14 | # The robots will never reach their goal points so set iteration number 15 | iterations = 3000 16 | 17 | # Define goal points outside of the arena 18 | goal_points = np.array(np.mat('5 5 5 5 5; 5 5 5 5 5; 0 0 0 0 0')) 19 | 20 | # Create unicycle position controller 21 | unicycle_position_controller = create_clf_unicycle_position_controller() 22 | 23 | # Create barrier certificates to avoid collision 24 | uni_barrier_cert = create_unicycle_differential_drive_barrier_certificate_with_boundary() 25 | 26 | 27 | # define x initially 28 | x = r.get_poses() 29 | r.step() 30 | 31 | # While the number of robots at the required poses is less 32 | # than N... 33 | for i in range(iterations): 34 | 35 | # Get poses of agents 36 | x = r.get_poses() 37 | 38 | # Create single-integrator control inputs 39 | dxu = unicycle_position_controller(x, goal_points[:2][:]) 40 | 41 | # Create safe control inputs (i.e., no collisions) 42 | dxu = uni_barrier_cert(dxu, x) 43 | 44 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 45 | r.set_velocities(np.arange(N), dxu) 46 | 47 | # Iterate the simulation 48 | r.step() 49 | -------------------------------------------------------------------------------- /rps/examples/consensus/consensus.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.graph import * 3 | from rps.utilities.transformations import * 4 | from rps.utilities.barrier_certificates import * 5 | from rps.utilities.misc import * 6 | from rps.utilities.controllers import * 7 | 8 | import numpy as np 9 | 10 | # Instantiate Robotarium object 11 | N = 12 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 13 | 14 | # How many iterations do we want (about N*0.033 seconds) 15 | iterations = 1000 16 | 17 | # We're working in single-integrator dynamics, and we don't want the robots 18 | # to collide or drive off the testbed. Thus, we're going to use barrier certificates 19 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 20 | 21 | # Create SI to UNI dynamics tranformation 22 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 23 | 24 | # Generated a connected graph Laplacian (for a cylce graph). 25 | L = cycle_GL(N) 26 | 27 | for k in range(iterations): 28 | 29 | # Get the poses of the robots and convert to single-integrator poses 30 | x = r.get_poses() 31 | x_si = uni_to_si_states(x) 32 | 33 | # Initialize the single-integrator control inputs 34 | si_velocities = np.zeros((2, N)) 35 | 36 | # For each robot... 37 | for i in range(N): 38 | # Get the neighbors of robot 'i' (encoded in the graph Laplacian) 39 | j = topological_neighbors(L, i) 40 | # Compute the consensus algorithm 41 | si_velocities[:, i] = np.sum(x_si[:, j] - x_si[:, i, None], 1) 42 | 43 | # Use the barrier certificate to avoid collisions 44 | si_velocities = si_barrier_cert(si_velocities, x_si) 45 | 46 | # Transform single integrator to unicycle 47 | dxu = si_to_uni_dyn(si_velocities, x) 48 | 49 | # Set the velocities of agents 1,...,N 50 | r.set_velocities(np.arange(N), dxu) 51 | # Iterate the simulation 52 | r.step() 53 | 54 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 55 | r.call_at_scripts_end() 56 | -------------------------------------------------------------------------------- /rps/examples/consensus/consensus_fewer_errors.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.graph import * 3 | from rps.utilities.transformations import * 4 | from rps.utilities.barrier_certificates import * 5 | from rps.utilities.misc import * 6 | from rps.utilities.controllers import * 7 | 8 | import numpy as np 9 | 10 | # Instantiate Robotarium object 11 | N = 12 12 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 13 | 14 | # How many iterations do we want (about N*0.033 seconds) 15 | iterations = 1000 16 | 17 | #Maximum linear speed of robot specified by motors 18 | magnitude_limit = 0.15 19 | 20 | # We're working in single-integrator dynamics, and we don't want the robots 21 | # to collide or drive off the testbed. Thus, we're going to use barrier certificates 22 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 23 | 24 | # Create SI to UNI dynamics tranformation 25 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 26 | 27 | # Generated a connected graph Laplacian (for a cylce graph). 28 | L = cycle_GL(N) 29 | 30 | for k in range(iterations): 31 | 32 | # Get the poses of the robots and convert to single-integrator poses 33 | x = r.get_poses() 34 | x_si = uni_to_si_states(x) 35 | 36 | # Initialize the single-integrator control inputs 37 | si_velocities = np.zeros((2, N)) 38 | 39 | # For each robot... 40 | for i in range(N): 41 | # Get the neighbors of robot 'i' (encoded in the graph Laplacian) 42 | j = topological_neighbors(L, i) 43 | # Compute the consensus algorithm 44 | si_velocities[:, i] = np.sum(x_si[:, j] - x_si[:, i, None], 1) 45 | 46 | 47 | #Keep single integrator control vectors under specified magnitude 48 | # Threshold control inputs 49 | norms = np.linalg.norm(dxi, 2, 0) 50 | idxs_to_normalize = (norms > magnitude_limit) 51 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 52 | 53 | # Use the barrier certificate to avoid collisions 54 | si_velocities = si_barrier_cert(si_velocities, x_si) 55 | 56 | # Transform single integrator to unicycle 57 | dxu = si_to_uni_dyn(si_velocities, x) 58 | 59 | # Set the velocities of agents 1,...,N 60 | r.set_velocities(np.arange(N), dxu) 61 | # Iterate the simulation 62 | r.step() 63 | 64 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 65 | r.call_at_scripts_end() 66 | -------------------------------------------------------------------------------- /rps/examples/data_saving/leader_follower_save_data.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Same script as leader_follower.m but with additional data saving. Two 3 | data sets will be saved, one saving the distance between connected robots 4 | through time, and another with the distance between the leader and goal 5 | location when the goal is "reached". They will each be saved as .npy files 6 | and human readable csv .txt files. 7 | 8 | Sean Wilson 9 | 10/2019 10 | ''' 11 | 12 | #Import Robotarium Utilities 13 | import rps.robotarium as robotarium 14 | from rps.utilities.transformations import * 15 | from rps.utilities.graph import * 16 | from rps.utilities.barrier_certificates import * 17 | from rps.utilities.misc import * 18 | from rps.utilities.controllers import * 19 | 20 | #Other Imports 21 | import numpy as np 22 | import time 23 | 24 | # Experiment Constants 25 | iterations = 5000 #Run the simulation/experiment for 5000 steps (5000*0.033 ~= 2min 45sec) 26 | N=4 #Number of robots to use, this must stay 4 unless the Laplacian is changed. 27 | 28 | waypoints = np.array([[-1, -1, 1, 1],[0.8, -0.8, -0.8, 0.8]]) #Waypoints the leader moves to. 29 | close_enough = 0.03; #How close the leader must get to the waypoint to move to the next one. 30 | 31 | # Preallocate data saving 32 | robot_distance = np.zeros((5,iterations)) #Saving 4 inter-robot distances and time 33 | goal_distance = np.empty((0,2)) 34 | start_time = time.time() 35 | 36 | # Create the desired Laplacian 37 | followers = -completeGL(N-1) 38 | L = np.zeros((N,N)) 39 | L[1:N,1:N] = followers 40 | L[1,1] = L[1,1] + 1 41 | L[1,0] = -1 42 | 43 | # Find connections 44 | [rows,cols] = np.where(L==1) 45 | 46 | # For computational/memory reasons, initialize the velocity vector 47 | dxi = np.zeros((2,N)) 48 | 49 | #Initialize leader state 50 | state = 0 51 | 52 | #Limit maximum linear speed of any robot 53 | magnitude_limit = 0.15 54 | 55 | # Create gains for our formation control algorithm 56 | formation_control_gain = 10 57 | desired_distance = 0.3 58 | 59 | # Initial Conditions to Avoid Barrier Use in the Beginning. 60 | initial_conditions = np.array([[0, 0.5, 0.3, -0.1],[0.5, 0.5, 0.2, 0],[0, 0, 0, 0]]) 61 | 62 | # Instantiate the Robotarium object with these parameters 63 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=False) 64 | 65 | # Grab Robotarium tools to do simgle-integrator to unicycle conversions and collision avoidance 66 | # Single-integrator -> unicycle dynamics mapping 67 | si_to_uni_dyn ,uni_to_si_states = create_si_to_uni_mapping() 68 | # Single-integrator barrier certificates 69 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 70 | # Single-integrator position controller 71 | leader_controller = create_si_position_controller(velocity_magnitude_limit=0.1) 72 | 73 | for t in range(iterations): 74 | 75 | # Get the most recent pose information from the Robotarium. The time delay is 76 | # approximately 0.033s 77 | x = r.get_poses() 78 | xi = uni_to_si_states(x) 79 | 80 | #Algorithm 81 | 82 | #Followers 83 | for i in range(1,N): 84 | # Zero velocities and get the topological neighbors of agent i 85 | dxi[:,[i]]=np.zeros((2,1)) 86 | neighbors = topological_neighbors(L,i) 87 | 88 | for j in neighbors: 89 | dxi[:,[i]] += formation_control_gain*(np.power(np.linalg.norm(x[:2,[j]]-x[:2,[i]]), 2)-np.power(desired_distance, 2))*(x[:2,[j]]-x[:2,[i]]) 90 | 91 | #Leader 92 | waypoint = waypoints[:,state].reshape((2,1)) 93 | 94 | dxi[:,[0]] = leader_controller(x[:2,[0]], waypoint) 95 | if np.linalg.norm(x[:2,[0]] - waypoint) < close_enough: 96 | state = (state + 1)%4 97 | goal_distance = np.append(goal_distance, np.array([[np.linalg.norm(xi[:,[0]] - waypoint)],[time.time()-start_time]])) 98 | 99 | 100 | #Keep single integrator control vectors under specified magnitude 101 | # Threshold control inputs 102 | norms = np.linalg.norm(dxi, 2, 0) 103 | idxs_to_normalize = (norms > magnitude_limit) 104 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 105 | 106 | #Use barriers and convert single-integrator to unicycle commands 107 | dxi = si_barrier_cert(dxi, x[:2,:]) 108 | dxu = si_to_uni_dyn(dxi,x) 109 | 110 | # Set the velocities of agents 1,...,N to dxu 111 | r.set_velocities(np.arange(N), dxu) 112 | 113 | # Compute data to be saved and stored in matrix 114 | # Distance between connected robots 115 | robot_distance[0,t] = np.linalg.norm(xi[:,[0]]-xi[:,[1]]) 116 | robot_distance[4,t] = time.time() - start_time 117 | for j in range(1,int(len(rows)/2)+1): 118 | robot_distance[j,t] = np.linalg.norm(xi[:,[rows[j]]]-xi[:,[cols[j]]]) 119 | 120 | # Iterate the simulation 121 | r.step() 122 | 123 | #Save Data Locally as Numpy 124 | np.save('goal_distance_data', goal_distance) 125 | np.save('inter_robot_distance_data', robot_distance) 126 | 127 | #Save Data Locally as CSV Text File 128 | np.savetxt('goal_distance_data.txt', goal_distance, delimiter=',') 129 | np.savetxt('inter_robot_distance_data.txt', robot_distance.T, delimiter=',') 130 | 131 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 132 | r.call_at_scripts_end() 133 | -------------------------------------------------------------------------------- /rps/examples/formation_control/formation_control.py: -------------------------------------------------------------------------------- 1 | #Import Robotarium Utilities 2 | import rps.robotarium as robotarium 3 | from rps.utilities.transformations import * 4 | from rps.utilities.graph import * 5 | from rps.utilities.barrier_certificates import * 6 | from rps.utilities.misc import * 7 | from rps.utilities.controllers import * 8 | 9 | # Array representing the geometric distances betwen the agents. In this case, 10 | # the agents try to form a Rectangle 11 | L = np.array([ 12 | [3, -1, 0, -1, 0, -1], 13 | [-1, 3, -1, 0, -1, 0], 14 | [0, -1, 3, -1, 0, -1], 15 | [-1, 0 , -1, 3, -1, 0], 16 | [0, -1, 0, -1, 3, -1], 17 | [-1, 0, -1, 0, -1, 3] 18 | ]) 19 | 20 | # Some gains for this experiment. These aren't incredibly relevant. 21 | d = 0.3 22 | ddiag = np.sqrt(5)*d 23 | formation_control_gain = 10 24 | 25 | # Weight matrix to control inter-agent distances 26 | weights = np.array([ 27 | [0, d, 0, d, 0, ddiag], 28 | [d, 0, d, 0, d, 0], 29 | [0, d, 0, ddiag, 0, d], 30 | [d, 0, ddiag, 0, d, 0], 31 | [0, d, 0, d, 0, d], 32 | [ddiag, 0, d, 0, d, 0] 33 | ]) 34 | 35 | # Experiment constants 36 | iterations = 2000 37 | N = 6 38 | 39 | #Limit maximum linear speed of any robot 40 | magnitude_limit = 0.15 41 | 42 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 43 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 44 | si_to_uni_dyn = create_si_to_uni_dynamics() 45 | 46 | for k in range(iterations): 47 | 48 | # Get the poses of the robots 49 | x = r.get_poses() 50 | 51 | # Initialize a velocity vector 52 | dxi = np.zeros((2, N)) 53 | 54 | for i in range(N): 55 | for j in topological_neighbors(L, i): 56 | # Perform a weighted consensus to make the rectangular shape 57 | error = x[:2, j] - x[:2, i] 58 | dxi[:, i] += formation_control_gain*(np.power(np.linalg.norm(error), 2)- np.power(weights[i, j], 2)) * error 59 | 60 | #Keep single integrator control vectors under specified magnitude 61 | # Threshold control inputs 62 | norms = np.linalg.norm(dxi, 2, 0) 63 | idxs_to_normalize = (norms > magnitude_limit) 64 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 65 | 66 | # Make sure that the robots don't collide 67 | dxi = si_barrier_cert(dxi, x[:2, :]) 68 | 69 | # Transform the single-integrator dynamcis to unicycle dynamics 70 | dxu = si_to_uni_dyn(dxi, x) 71 | 72 | # Set the velocities of the robots 73 | r.set_velocities(np.arange(N), dxu) 74 | # Iterate the simulation 75 | r.step() 76 | 77 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 78 | r.call_at_scripts_end() -------------------------------------------------------------------------------- /rps/examples/go_to_point/si_go_to_point.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | 14 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=False) 15 | 16 | # Define goal points by removing orientation from poses 17 | goal_points = generate_initial_conditions(N) 18 | 19 | # Create single integrator position controller 20 | single_integrator_position_controller = create_si_position_controller() 21 | 22 | # Create barrier certificates to avoid collision 23 | #si_barrier_cert = create_single_integrator_barrier_certificate() 24 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 25 | 26 | _, uni_to_si_states = create_si_to_uni_mapping() 27 | 28 | # Create mapping from single integrator velocity commands to unicycle velocity commands 29 | si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion() 30 | 31 | # define x initially 32 | x = r.get_poses() 33 | x_si = uni_to_si_states(x) 34 | r.step() 35 | 36 | # While the number of robots at the required poses is less 37 | # than N... 38 | while (np.size(at_pose(np.vstack((x_si,x[2,:])), goal_points, rotation_error=100)) != N): 39 | 40 | # Get poses of agents 41 | x = r.get_poses() 42 | x_si = uni_to_si_states(x) 43 | 44 | # Create single-integrator control inputs 45 | dxi = single_integrator_position_controller(x_si, goal_points[:2][:]) 46 | 47 | # Create safe control inputs (i.e., no collisions) 48 | dxi = si_barrier_cert(dxi, x_si) 49 | 50 | # Transform single integrator velocity commands to unicycle 51 | dxu = si_to_uni_dyn(dxi, x) 52 | 53 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 54 | r.set_velocities(np.arange(N), dxu) 55 | # Iterate the simulation 56 | r.step() 57 | 58 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 59 | r.call_at_scripts_end() 60 | -------------------------------------------------------------------------------- /rps/examples/go_to_point/uni_go_to_point.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 14 | 15 | # Define goal points by removing orientation from poses 16 | goal_points = generate_initial_conditions(N) 17 | 18 | # Create unicycle position controller 19 | unicycle_position_controller = create_clf_unicycle_position_controller() 20 | 21 | # Create barrier certificates to avoid collision 22 | uni_barrier_cert = create_unicycle_barrier_certificate() 23 | 24 | # define x initially 25 | x = r.get_poses() 26 | r.step() 27 | 28 | # While the number of robots at the required poses is less 29 | # than N... 30 | while (np.size(at_pose(x, goal_points, rotation_error=100)) != N): 31 | 32 | # Get poses of agents 33 | x = r.get_poses() 34 | 35 | # Create single-integrator control inputs 36 | dxu = unicycle_position_controller(x, goal_points[:2][:]) 37 | 38 | # Create safe control inputs (i.e., no collisions) 39 | dxu = uni_barrier_cert(dxu, x) 40 | 41 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 42 | r.set_velocities(np.arange(N), dxu) 43 | 44 | # Iterate the simulation 45 | r.step() 46 | 47 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 48 | r.call_at_scripts_end() 49 | -------------------------------------------------------------------------------- /rps/examples/go_to_pose/uni_go_to_pose_clf.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 14 | 15 | # Define goal points by removing orientation from poses 16 | goal_points = generate_initial_conditions(N) 17 | 18 | # Create unicycle pose controller 19 | unicycle_pose_controller = create_clf_unicycle_pose_controller() 20 | 21 | # Create barrier certificates to avoid collision 22 | uni_barrier_cert = create_unicycle_barrier_certificate() 23 | 24 | # define x initially 25 | x = r.get_poses() 26 | r.step() 27 | 28 | # While the number of robots at the required poses is less 29 | # than N... 30 | while (np.size(at_pose(x, goal_points)) != N): 31 | 32 | # Get poses of agents 33 | x = r.get_poses() 34 | 35 | # Create unicycle control inputs 36 | dxu = unicycle_pose_controller(x, goal_points) 37 | 38 | # Create safe control inputs (i.e., no collisions) 39 | dxu = uni_barrier_cert(dxu, x) 40 | 41 | # Set the velocities 42 | r.set_velocities(np.arange(N), dxu) 43 | 44 | # Iterate the simulation 45 | r.step() 46 | 47 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 48 | r.call_at_scripts_end() 49 | -------------------------------------------------------------------------------- /rps/examples/go_to_pose/uni_go_to_pose_hybrid.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 14 | 15 | # Define goal points by removing orientation from poses 16 | goal_points = generate_initial_conditions(N) 17 | 18 | # Create unicycle pose controller 19 | unicycle_pose_controller = create_hybrid_unicycle_pose_controller() 20 | 21 | # Create barrier certificates to avoid collision 22 | uni_barrier_cert = create_unicycle_barrier_certificate() 23 | 24 | # define x initially 25 | x = r.get_poses() 26 | r.step() 27 | 28 | # While the number of robots at the required poses is less 29 | # than N... 30 | while (np.size(at_pose(x, goal_points)) != N): 31 | 32 | # Get poses of agents 33 | x = r.get_poses() 34 | 35 | # Create unicycle control inputs 36 | dxu = unicycle_pose_controller(x, goal_points) 37 | 38 | # Create safe control inputs (i.e., no collisions) 39 | dxu = uni_barrier_cert(dxu, x) 40 | 41 | # Set the velocities 42 | r.set_velocities(np.arange(N), dxu) 43 | 44 | # Iterate the simulation 45 | r.step() 46 | 47 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 48 | r.call_at_scripts_end() 49 | -------------------------------------------------------------------------------- /rps/examples/leader_follower_static/leader_follower.py: -------------------------------------------------------------------------------- 1 | ''' 2 | TODO: UPDATE DESCRIPTION 3 | 4 | Sean Wilson 5 | 10/2019 6 | ''' 7 | 8 | #Import Robotarium Utilities 9 | import rps.robotarium as robotarium 10 | from rps.utilities.transformations import * 11 | from rps.utilities.graph import * 12 | from rps.utilities.barrier_certificates import * 13 | from rps.utilities.misc import * 14 | from rps.utilities.controllers import * 15 | 16 | #Other Imports 17 | import numpy as np 18 | 19 | # Experiment Constants 20 | iterations = 5000 #Run the simulation/experiment for 5000 steps (5000*0.033 ~= 2min 45sec) 21 | N=4 #Number of robots to use, this must stay 4 unless the Laplacian is changed. 22 | 23 | waypoints = np.array([[-1, -1, 1, 1],[0.8, -0.8, -0.8, 0.8]]) #Waypoints the leader moves to. 24 | close_enough = 0.03; #How close the leader must get to the waypoint to move to the next one. 25 | 26 | # Create the desired Laplacian 27 | followers = -completeGL(N-1) 28 | L = np.zeros((N,N)) 29 | L[1:N,1:N] = followers 30 | L[1,1] = L[1,1] + 1 31 | L[1,0] = -1 32 | 33 | # Find connections 34 | [rows,cols] = np.where(L==1) 35 | 36 | # For computational/memory reasons, initialize the velocity vector 37 | dxi = np.zeros((2,N)) 38 | 39 | #Initialize leader state 40 | state = 0 41 | 42 | #Limit maximum linear speed of any robot 43 | magnitude_limit = 0.15 44 | 45 | # Create gains for our formation control algorithm 46 | formation_control_gain = 10 47 | desired_distance = 0.3 48 | 49 | # Initial Conditions to Avoid Barrier Use in the Beginning. 50 | initial_conditions = np.array([[0, 0.5, 0.3, -0.1],[0.5, 0.5, 0.2, 0],[0, 0, 0, 0]]) 51 | 52 | # Instantiate the Robotarium object with these parameters 53 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=True) 54 | 55 | # Grab Robotarium tools to do simgle-integrator to unicycle conversions and collision avoidance 56 | # Single-integrator -> unicycle dynamics mapping 57 | _,uni_to_si_states = create_si_to_uni_mapping() 58 | si_to_uni_dyn = create_si_to_uni_dynamics() 59 | # Single-integrator barrier certificates 60 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 61 | # Single-integrator position controller 62 | leader_controller = create_si_position_controller(velocity_magnitude_limit=0.1) 63 | 64 | 65 | for t in range(iterations): 66 | 67 | # Get the most recent pose information from the Robotarium. The time delay is 68 | # approximately 0.033s 69 | x = r.get_poses() 70 | xi = uni_to_si_states(x) 71 | 72 | #Algorithm 73 | 74 | #Followers 75 | for i in range(1,N): 76 | # Zero velocities and get the topological neighbors of agent i 77 | dxi[:,[i]]=np.zeros((2,1)) 78 | neighbors = topological_neighbors(L,i) 79 | 80 | for j in neighbors: 81 | dxi[:,[i]] += formation_control_gain*(np.power(np.linalg.norm(x[:2,[j]]-x[:2,[i]]), 2)-np.power(desired_distance, 2))*(x[:2,[j]]-x[:2,[i]]) 82 | 83 | #Leader 84 | waypoint = waypoints[:,state].reshape((2,1)) 85 | 86 | dxi[:,[0]] = leader_controller(x[:2,[0]], waypoint) 87 | if np.linalg.norm(x[:2,[0]] - waypoint) < close_enough: 88 | state = (state + 1)%4 89 | 90 | #Keep single integrator control vectors under specified magnitude 91 | # Threshold control inputs 92 | norms = np.linalg.norm(dxi, 2, 0) 93 | idxs_to_normalize = (norms > magnitude_limit) 94 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 95 | 96 | #Use barriers and convert single-integrator to unicycle commands 97 | dxi = si_barrier_cert(dxi, x[:2,:]) 98 | dxu = si_to_uni_dyn(dxi,x) 99 | 100 | # Set the velocities of agents 1,...,N to dxu 101 | r.set_velocities(np.arange(N), dxu) 102 | 103 | # Iterate the simulation 104 | r.step() 105 | 106 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 107 | r.call_at_scripts_end() 108 | -------------------------------------------------------------------------------- /rps/examples/plotting/GTLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/rps/examples/plotting/GTLogo.png -------------------------------------------------------------------------------- /rps/examples/plotting/barrier_certificates_with_plotting.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # How many robots we want to use in the simulation 11 | N = 10 12 | # Instantiate the Robotarium object with these parameters 13 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, sim_in_real_time=True) 14 | 15 | # How many times should the robots form the circle? 16 | num_cycles=2 17 | count = -1 # How many times have they formed the circle? (starts at -1 since initial formation will increment the count) 18 | 19 | # Default Barrier Parameters 20 | safety_radius = 0.17 21 | 22 | # Plotting Parameters 23 | CM = np.random.rand(N,3) # Random Colors 24 | safety_radius_marker_size = determine_marker_size(r,safety_radius) # Will scale the plotted markers to be the diameter of provided argument (in meters) 25 | font_height_meters = 0.1 26 | font_height_points = determine_font_size(r,font_height_meters) # Will scale the plotted font height to that of the provided argument (in meters) 27 | 28 | # Initial plots 29 | x=r.get_poses() 30 | g = r.axes.scatter(x[0,:], x[1,:], s=np.pi/4*safety_radius_marker_size, marker='o', facecolors='none',edgecolors=CM,linewidth=7) 31 | #g = r.axes.plot(x[0,:], x[1,:], markersize=safety_radius_marker_size, linestyle='none', marker='o', markerfacecolor='none', markeredgecolor=color[CM],linewidth=7) 32 | r.step() 33 | 34 | # We're working in single-integrator dynamics, and we don't want the robots 35 | # to collide. Thus, we're going to use barrier certificates 36 | si_barrier_cert = create_single_integrator_barrier_certificate() 37 | 38 | # Create single integrator position controller 39 | si_position_controller = create_si_position_controller() 40 | 41 | # Create SI to UNI dynamics tranformation 42 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 43 | 44 | # This portion of the code generates points on a circle enscribed in a 6x6 square 45 | # that's centered on the origin. The robots switch positions on the circle. 46 | xybound = np.array([-1.2, 1.2, -1, 1]) 47 | p_theta = 2*np.pi*(np.arange(0, 2*N, 2)/(2*N)) 48 | p_circ = np.vstack([ 49 | np.hstack([xybound[1]*np.cos(p_theta), xybound[1]*np.cos(p_theta+np.pi)]), 50 | np.hstack([xybound[3]*np.sin(p_theta), xybound[3]*np.sin(p_theta+np.pi)]) 51 | ]) 52 | 53 | # These variables are so we can tell when the robots should switch positions 54 | # on the circle. 55 | flag = 0 56 | x_goal = p_circ[:, :N] 57 | 58 | # Perform the simulation for a certain number of iterations 59 | while(1): 60 | 61 | # Get the poses of the agents that we want 62 | x = r.get_poses() 63 | 64 | # Update Plotted Visualization 65 | g.set_offsets(x[:2,:].T) 66 | # This updates the marker sizes if the figure window size is changed. 67 | # This should be removed when submitting to the Robotarium. 68 | g.set_sizes([determine_marker_size(r,safety_radius)]) 69 | 70 | # We're planning on using the single-integrator to unciycle mapping, 71 | # so our single-integrator states are the projected point. 72 | x_si = uni_to_si_states(x) 73 | 74 | # Initialize a velocities variable 75 | si_velocities = np.zeros((2, N)) 76 | 77 | # Check if all the agents are close enough to the goals 78 | if(np.linalg.norm(x_goal - x_si) < 0.05): 79 | flag = 1-flag 80 | count += 1 81 | 82 | if count == num_cycles: 83 | break 84 | 85 | # Switch goals depending on the state of flag (goals switch to opposite 86 | # sides of the circle) 87 | if(flag == 0): 88 | x_goal = p_circ[:, :N] 89 | else: 90 | x_goal = p_circ[:, N:] 91 | 92 | # Use a position controller to drive to the goal position 93 | dxi = si_position_controller(x_si,x_goal) 94 | 95 | # Use the barrier certificates to make sure that the agents don't collide 96 | dxi = si_barrier_cert(dxi, x_si) 97 | 98 | # Use the second single-integrator-to-unicycle mapping to map to unicycle 99 | # dynamics 100 | dxu = si_to_uni_dyn(dxi, x) 101 | 102 | # Set the velocities of agents 1,...,N to dxu 103 | r.set_velocities(np.arange(N), dxu) 104 | # Iterate the simulation 105 | r.step() 106 | 107 | finished_caption = "All robots safely reached \n their destination" 108 | finished_label = r.axes.text(0,0,finished_caption,fontsize=font_height_points, color='k',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=20) 109 | r.step() 110 | time.sleep(5) 111 | 112 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 113 | r.call_at_scripts_end() 114 | -------------------------------------------------------------------------------- /rps/examples/plotting/leader_follower_with_plotting.py: -------------------------------------------------------------------------------- 1 | ''' 2 | TODO: UPDATE DESCRIPTION 3 | 4 | Sean Wilson 5 | 10/2019 6 | ''' 7 | 8 | #Import Robotarium Utilities 9 | import rps.robotarium as robotarium 10 | from rps.utilities.transformations import * 11 | from rps.utilities.graph import * 12 | from rps.utilities.barrier_certificates import * 13 | from rps.utilities.misc import * 14 | from rps.utilities.controllers import * 15 | 16 | #Other Imports 17 | import numpy as np 18 | 19 | # Experiment Constants 20 | iterations = 5000 #Run the simulation/experiment for 5000 steps (5000*0.033 ~= 2min 45sec) 21 | N=4 #Number of robots to use, this must stay 4 unless the Laplacian is changed. 22 | 23 | waypoints = np.array([[-1, -1, 1, 1],[0.8, -0.8, -0.8, 0.8]]) #Waypoints the leader moves to. 24 | close_enough = 0.03; #How close the leader must get to the waypoint to move to the next one. 25 | 26 | # Create the desired Laplacian 27 | followers = -completeGL(N-1) 28 | L = np.zeros((N,N)) 29 | L[1:N,1:N] = followers 30 | L[1,1] = L[1,1] + 1 31 | L[1,0] = -1 32 | 33 | # Find connections 34 | [rows,cols] = np.where(L==1) 35 | 36 | # For computational/memory reasons, initialize the velocity vector 37 | dxi = np.zeros((2,N)) 38 | 39 | #Initialize leader state 40 | state = 0 41 | 42 | #Limit maximum linear speed of any robot 43 | magnitude_limit = 0.15 44 | 45 | # Create gains for our formation control algorithm 46 | formation_control_gain = 10 47 | desired_distance = 0.3 48 | 49 | # Initial Conditions to Avoid Barrier Use in the Beginning. 50 | initial_conditions = np.array([[0, 0.5, 0.3, -0.1],[0.5, 0.5, 0.2, 0],[0, 0, 0, 0]]) 51 | 52 | # Instantiate the Robotarium object with these parameters 53 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=True) 54 | 55 | # Grab Robotarium tools to do simgle-integrator to unicycle conversions and collision avoidance 56 | # Single-integrator -> unicycle dynamics mapping 57 | _,uni_to_si_states = create_si_to_uni_mapping() 58 | si_to_uni_dyn = create_si_to_uni_dynamics(angular_velocity_limit=np.pi/2) 59 | # Single-integrator barrier certificates 60 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 61 | # Single-integrator position controller 62 | leader_controller = create_si_position_controller(velocity_magnitude_limit=0.15) 63 | 64 | # Plotting Parameters 65 | CM = np.random.rand(N,3) # Random Colors 66 | marker_size_goal = determine_marker_size(r,0.2) 67 | font_size_m = 0.1 68 | font_size = determine_font_size(r,font_size_m) 69 | line_width = 5 70 | 71 | # Create goal text and markers 72 | 73 | #Text with goal identification 74 | goal_caption = ['G{0}'.format(ii) for ii in range(waypoints.shape[1])] 75 | #Plot text for caption 76 | waypoint_text = [r.axes.text(waypoints[0,ii], waypoints[1,ii], goal_caption[ii], fontsize=font_size, color='k',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=-2) 77 | for ii in range(waypoints.shape[1])] 78 | g = [r.axes.scatter(waypoints[0,ii], waypoints[1,ii], s=marker_size_goal, marker='s', facecolors='none',edgecolors=CM[ii,:],linewidth=line_width,zorder=-2) 79 | for ii in range(waypoints.shape[1])] 80 | 81 | # Plot Graph Connections 82 | x = r.get_poses() # Need robot positions to do this. 83 | linked_follower_index = np.empty((2,3)) 84 | follower_text = np.empty((3,0)) 85 | for jj in range(1,int(len(rows)/2)+1): 86 | linked_follower_index[:,[jj-1]] = np.array([[rows[jj]],[cols[jj]]]) 87 | follower_text = np.append(follower_text,'{0}'.format(jj)) 88 | 89 | line_follower = [r.axes.plot([x[0,rows[kk]], x[0,cols[kk]]],[x[1,rows[kk]], x[1,cols[kk]]],linewidth=line_width,color='b',zorder=-1) 90 | for kk in range(1,N)] 91 | line_leader = r.axes.plot([x[0,0],x[0,1]],[x[1,0],x[1,1]],linewidth=line_width,color='r',zorder = -1) 92 | follower_labels = [r.axes.text(x[0,kk],x[1,kk]+0.15,follower_text[kk-1],fontsize=font_size, color='b',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=0) 93 | for kk in range(1,N)] 94 | leader_label = r.axes.text(x[0,0],x[1,0]+0.15,"Leader",fontsize=font_size, color='r',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=0) 95 | 96 | r.step() 97 | for t in range(iterations): 98 | 99 | # Get the most recent pose information from the Robotarium. The time delay is 100 | # approximately 0.033s 101 | x = r.get_poses() 102 | xi = uni_to_si_states(x) 103 | 104 | # Update Plot Handles 105 | for q in range(N-1): 106 | follower_labels[q].set_position([xi[0,q+1],xi[1,q+1]+0.15]) 107 | follower_labels[q].set_fontsize(determine_font_size(r,font_size_m)) 108 | line_follower[q][0].set_data([x[0,rows[q+1]], x[0,cols[q+1]]],[x[1,rows[q+1]], x[1,cols[q+1]]]) 109 | leader_label.set_position([xi[0,0],xi[1,0]+0.15]) 110 | leader_label.set_fontsize(determine_font_size(r,font_size_m)) 111 | line_leader[0].set_data([x[0,0],x[0,1]],[x[1,0],x[1,1]]) 112 | 113 | # This updates the marker sizes if the figure window size is changed. 114 | # This should be removed when submitting to the Robotarium. 115 | for q in range(waypoints.shape[1]): 116 | waypoint_text[q].set_fontsize(determine_font_size(r,font_size_m)) 117 | g[q].set_sizes([determine_marker_size(r,0.2)]) 118 | 119 | #Algorithm 120 | 121 | #Followers 122 | for i in range(1,N): 123 | # Zero velocities and get the topological neighbors of agent i 124 | dxi[:,[i]]=np.zeros((2,1)) 125 | neighbors = topological_neighbors(L,i) 126 | 127 | for j in neighbors: 128 | dxi[:,[i]] += formation_control_gain*(np.power(np.linalg.norm(x[:2,[j]]-x[:2,[i]]), 2)-np.power(desired_distance, 2))*(x[:2,[j]]-x[:2,[i]]) 129 | 130 | #Leader 131 | waypoint = waypoints[:,state].reshape((2,1)) 132 | 133 | dxi[:,[0]] = leader_controller(x[:2,[0]], waypoint) 134 | if np.linalg.norm(x[:2,[0]] - waypoint) < close_enough: 135 | state = (state + 1)%4 136 | 137 | #Keep single integrator control vectors under specified magnitude 138 | # Threshold control inputs 139 | norms = np.linalg.norm(dxi, 2, 0) 140 | idxs_to_normalize = (norms > magnitude_limit) 141 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 142 | 143 | #Use barriers and convert single-integrator to unicycle commands 144 | dxi = si_barrier_cert(dxi, x[:2,:]) 145 | dxu = si_to_uni_dyn(dxi,x) 146 | 147 | # Set the velocities of agents 1,...,N to dxu 148 | r.set_velocities(np.arange(N), dxu) 149 | 150 | # Iterate the simulation 151 | r.step() 152 | 153 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 154 | r.call_at_scripts_end() 155 | -------------------------------------------------------------------------------- /rps/examples/plotting/si_go_to_point_gt.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 4 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0; 0.8 -0.3 -0.75 0.1; 0 0 0 0')) 13 | 14 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=False) 15 | 16 | # Define goal points by removing orientation from poses 17 | goal_points = 0.9*np.array([[-1, 1, -1, 1],[-1, -1, 1, 1],[0,0,0,0]]) 18 | 19 | # Create single integrator position controller 20 | single_integrator_position_controller = create_si_position_controller() 21 | 22 | # Create barrier certificates to avoid collision 23 | #si_barrier_cert = create_single_integrator_barrier_certificate() 24 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 25 | 26 | _, uni_to_si_states = create_si_to_uni_mapping() 27 | 28 | # Create mapping from single integrator velocity commands to unicycle velocity commands 29 | si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion() 30 | 31 | #Read in and scale image 32 | gt_img = plt.imread('GTLogo.png') 33 | x_img = np.linspace(-1.0, 1.0, gt_img.shape[1]) 34 | y_img = np.linspace(-1.0, 1.0, gt_img.shape[0]) 35 | 36 | gt_img_handle = r.axes.imshow(gt_img, extent=(-1, 1, -1, 1)) 37 | 38 | # define x initially 39 | x = r.get_poses() 40 | x_si = uni_to_si_states(x) 41 | r.step() 42 | 43 | # While the number of robots at the required poses is less 44 | # than N... 45 | while (np.size(at_pose(np.vstack((x_si,x[2,:])), goal_points, rotation_error=100)) != N): 46 | 47 | # Get poses of agents 48 | x = r.get_poses() 49 | x_si = uni_to_si_states(x) 50 | 51 | # Create single-integrator control inputs 52 | dxi = single_integrator_position_controller(x_si, goal_points[:2][:]) 53 | 54 | # Create safe control inputs (i.e., no collisions) 55 | dxi = si_barrier_cert(dxi, x_si) 56 | 57 | # Transform single integrator velocity commands to unicycle 58 | dxu = si_to_uni_dyn(dxi, x) 59 | 60 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 61 | r.set_velocities(np.arange(N), dxu) 62 | # Iterate the simulation 63 | r.step() 64 | 65 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 66 | r.call_at_scripts_end() 67 | -------------------------------------------------------------------------------- /rps/examples/plotting/si_go_to_point_with_plotting.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | 14 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=False) 15 | 16 | # Define goal points by removing orientation from poses 17 | goal_points = generate_initial_conditions(N, width=r.boundaries[2]-2*r.robot_diameter, height = r.boundaries[3]-2*r.robot_diameter, spacing=0.5) 18 | 19 | # Create single integrator position controller 20 | single_integrator_position_controller = create_si_position_controller() 21 | 22 | # Create barrier certificates to avoid collision 23 | #si_barrier_cert = create_single_integrator_barrier_certificate() 24 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary() 25 | 26 | _, uni_to_si_states = create_si_to_uni_mapping() 27 | 28 | # Create mapping from single integrator velocity commands to unicycle velocity commands 29 | si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion() 30 | 31 | # define x initially 32 | x = r.get_poses() 33 | x_si = uni_to_si_states(x) 34 | 35 | # Plotting Parameters 36 | CM = np.random.rand(N,3) # Random Colors 37 | goal_marker_size_m = 0.2 38 | robot_marker_size_m = 0.15 39 | marker_size_goal = determine_marker_size(r,goal_marker_size_m) 40 | marker_size_robot = determine_marker_size(r, robot_marker_size_m) 41 | font_size = determine_font_size(r,0.1) 42 | line_width = 5 43 | 44 | # Create Goal Point Markers 45 | #Text with goal identification 46 | goal_caption = ['G{0}'.format(ii) for ii in range(goal_points.shape[1])] 47 | #Plot text for caption 48 | goal_points_text = [r.axes.text(goal_points[0,ii], goal_points[1,ii], goal_caption[ii], fontsize=font_size, color='k',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=-2) 49 | for ii in range(goal_points.shape[1])] 50 | goal_markers = [r.axes.scatter(goal_points[0,ii], goal_points[1,ii], s=marker_size_goal, marker='s', facecolors='none',edgecolors=CM[ii,:],linewidth=line_width,zorder=-2) 51 | for ii in range(goal_points.shape[1])] 52 | robot_markers = [r.axes.scatter(x[0,ii], x[1,ii], s=marker_size_robot, marker='o', facecolors='none',edgecolors=CM[ii,:],linewidth=line_width) 53 | for ii in range(goal_points.shape[1])] 54 | 55 | 56 | 57 | r.step() 58 | 59 | # While the number of robots at the required poses is less 60 | # than N... 61 | while (np.size(at_pose(np.vstack((x_si,x[2,:])), goal_points, rotation_error=100)) != N): 62 | 63 | # Get poses of agents 64 | x = r.get_poses() 65 | x_si = uni_to_si_states(x) 66 | 67 | #Update Plot 68 | # Update Robot Marker Plotted Visualization 69 | for i in range(x.shape[1]): 70 | robot_markers[i].set_offsets(x[:2,i].T) 71 | # This updates the marker sizes if the figure window size is changed. 72 | # This should be removed when submitting to the Robotarium. 73 | robot_markers[i].set_sizes([determine_marker_size(r, robot_marker_size_m)]) 74 | 75 | for j in range(goal_points.shape[1]): 76 | goal_markers[j].set_sizes([determine_marker_size(r, goal_marker_size_m)]) 77 | 78 | # Create single-integrator control inputs 79 | dxi = single_integrator_position_controller(x_si, goal_points[:2][:]) 80 | 81 | # Create safe control inputs (i.e., no collisions) 82 | dxi = si_barrier_cert(dxi, x_si) 83 | 84 | # Transform single integrator velocity commands to unicycle 85 | dxu = si_to_uni_dyn(dxi, x) 86 | 87 | # Set the velocities by mapping the single-integrator inputs to unciycle inputs 88 | r.set_velocities(np.arange(N), dxu) 89 | # Iterate the simulation 90 | r.step() 91 | 92 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 93 | r.call_at_scripts_end() 94 | -------------------------------------------------------------------------------- /rps/examples/plotting/uni_go_to_pose_hybrid_with_plotting.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | # Instantiate Robotarium object 11 | N = 5 12 | initial_conditions = np.array(np.mat('1 0.5 -0.5 0 0.28; 0.8 -0.3 -0.75 0.1 0.34; 0 0 0 0 0')) 13 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 14 | 15 | # Define goal points by removing orientation from poses 16 | goal_points = generate_initial_conditions(N, width=r.boundaries[2]-2*r.robot_diameter, height = r.boundaries[3]-2*r.robot_diameter, spacing=0.5) 17 | 18 | # Create unicycle pose controller 19 | unicycle_pose_controller = create_hybrid_unicycle_pose_controller() 20 | 21 | # Create barrier certificates to avoid collision 22 | uni_barrier_cert = create_unicycle_barrier_certificate() 23 | 24 | # define x initially 25 | x = r.get_poses() 26 | 27 | # Plotting Parameters 28 | CM = np.random.rand(N,3) # Random Colors 29 | goal_marker_size_m = 0.2 30 | robot_marker_size_m = 0.15 31 | marker_size_goal = determine_marker_size(r,goal_marker_size_m) 32 | marker_size_robot = determine_marker_size(r, robot_marker_size_m) 33 | font_size = determine_font_size(r,0.1) 34 | line_width = 5 35 | 36 | # Create Goal Point Markers 37 | #Text with goal identification 38 | goal_caption = ['G{0}'.format(ii) for ii in range(goal_points.shape[1])] 39 | #Arrow for desired orientation 40 | goal_orientation_arrows = [r.axes.arrow(goal_points[0,ii], goal_points[1,ii], goal_marker_size_m*np.cos(goal_points[2,ii]), goal_marker_size_m*np.sin(goal_points[2,ii]), width = 0.02, length_includes_head=True, color = CM[ii,:], zorder=-2) 41 | for ii in range(goal_points.shape[1])] 42 | #Plot text for caption 43 | goal_points_text = [r.axes.text(goal_points[0,ii], goal_points[1,ii], goal_caption[ii], fontsize=font_size, color='k',fontweight='bold',horizontalalignment='center',verticalalignment='center',zorder=-3) 44 | for ii in range(goal_points.shape[1])] 45 | goal_markers = [r.axes.scatter(goal_points[0,ii], goal_points[1,ii], s=marker_size_goal, marker='s', facecolors='none',edgecolors=CM[ii,:],linewidth=line_width,zorder=-3) 46 | for ii in range(goal_points.shape[1])] 47 | robot_markers = [r.axes.scatter(x[0,ii], x[1,ii], s=marker_size_robot, marker='o', facecolors='none',edgecolors=CM[ii,:],linewidth=line_width) 48 | for ii in range(goal_points.shape[1])] 49 | 50 | r.step() 51 | 52 | # While the number of robots at the required poses is less 53 | # than N... 54 | while (np.size(at_pose(x, goal_points)) != N): 55 | 56 | # Get poses of agents 57 | x = r.get_poses() 58 | 59 | #Update Plot 60 | # Update Robot Marker Plotted Visualization 61 | for i in range(x.shape[1]): 62 | robot_markers[i].set_offsets(x[:2,i].T) 63 | # This updates the marker sizes if the figure window size is changed. 64 | # This should be removed when submitting to the Robotarium. 65 | robot_markers[i].set_sizes([determine_marker_size(r, robot_marker_size_m)]) 66 | 67 | for j in range(goal_points.shape[1]): 68 | goal_markers[j].set_sizes([determine_marker_size(r, goal_marker_size_m)]) 69 | 70 | # Create unicycle control inputs 71 | dxu = unicycle_pose_controller(x, goal_points) 72 | 73 | # Create safe control inputs (i.e., no collisions) 74 | dxu = uni_barrier_cert(dxu, x) 75 | 76 | # Set the velocities 77 | r.set_velocities(np.arange(N), dxu) 78 | 79 | # Iterate the simulation 80 | r.step() 81 | 82 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 83 | r.call_at_scripts_end() 84 | -------------------------------------------------------------------------------- /rps/examples/ra/ra.py: -------------------------------------------------------------------------------- 1 | import rps.robotarium as robotarium 2 | from rps.utilities.transformations import * 3 | from rps.utilities.barrier_certificates import * 4 | from rps.utilities.misc import * 5 | from rps.utilities.controllers import * 6 | 7 | import numpy as np 8 | import time 9 | 10 | def compute_a(states): 11 | state = states 12 | return np.array([0,0]) 13 | 14 | 15 | ## notice theta [-pi,pi] 16 | def compute_d(states): 17 | u = np.zeros(2) 18 | u[0] = 0 19 | u[1] = 1 20 | print(states[1,2]) 21 | return u 22 | 23 | # Instantiate Robotarium object 24 | N = 2 25 | # initial_conditions = np.array(np.mat('1 0.5; 0.8 -0.3 ; 0 0 ')) 26 | initial_conditions = np.array([[-1.5,0,0],[1,0,-np.pi]]).T 27 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions,sim_in_real_time=True) 28 | 29 | # Define goal points by removing orientation from poses 30 | goal_points = np.array(np.mat('0.5 1; -0.3 0.8 ; -0.6 0 ')) 31 | 32 | # generate_initial_conditions(N) 33 | 34 | # Create unicycle pose controller 35 | unicycle_pose_controller = create_clf_unicycle_pose_controller() 36 | 37 | # Create barrier certificates to avoid collision 38 | uni_barrier_cert = create_unicycle_barrier_certificate() 39 | 40 | # define x initially 41 | x = r.get_poses().T 42 | r.step() 43 | 44 | # While the number of robots at the required poses is less 45 | # than N... 46 | while (np.size(at_pose(x.T, goal_points)) != N): 47 | 48 | # Get poses of agents 49 | x = r.get_poses().T 50 | 51 | # print(x) 52 | # Create unicycle control inputs 53 | # dxu = unicycle_pose_controller(x.T, goal_points) 54 | # print(dxu) 55 | # # Create safe control inputs (i.e., no collisions) 56 | # dxu = uni_barrier_cert(dxu, x.T) 57 | dxu = np.zeros([2,2]) 58 | dxu[0] = compute_a(x) 59 | dxu[1] = compute_d(x) 60 | # # Set the velocities 61 | r.set_velocities(np.arange(N), dxu.T) 62 | # r.set_velocities(np.array([0,1]), np.array([[0.1,-0.1],[0.1,-0.1]]) ) 63 | # Iterate the simulation 64 | r.step() 65 | 66 | #Call at end of script to print debug information and for your script to run on the Robotarium server properly 67 | r.call_at_scripts_end() 68 | -------------------------------------------------------------------------------- /rps/robotarium.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | import matplotlib.patches as patches 7 | from rps.robotarium_abc import * 8 | 9 | # Robotarium This object provides routines to interface with the Robotarium. 10 | # 11 | # THIS CLASS SHOULD NEVER BE MODIFIED OR SUBMITTED 12 | 13 | class Robotarium(RobotariumABC): 14 | 15 | def __init__(self, number_of_robots=-1, show_figure=True, sim_in_real_time = True, initial_conditions=np.array([])): 16 | super().__init__(number_of_robots, show_figure, sim_in_real_time, initial_conditions) 17 | 18 | #Initialize some rendering variables 19 | self.previous_render_time = time.time() 20 | self.sim_in_real_time = sim_in_real_time 21 | 22 | #Initialize checks for step and get poses calls 23 | self._called_step_already = True 24 | self._checked_poses_already = False 25 | 26 | #Initialization of error collection. 27 | self._errors = {} 28 | 29 | #Initialize steps 30 | self._iterations = 0 31 | 32 | def get_poses(self): 33 | """Returns the states of the agents. 34 | 35 | -> 3xN numpy array (of robot poses) 36 | """ 37 | 38 | assert(not self._checked_poses_already), "Can only call get_poses() once per call of step()." 39 | # Allow step() to be called again. 40 | self._called_step_already = False 41 | self._checked_poses_already = True 42 | 43 | return self.poses 44 | 45 | def call_at_scripts_end(self): 46 | """Call this function at the end of scripts to display potentail errors. 47 | Even if you don't want to print the errors, calling this function at the 48 | end of your script will enable execution on the Robotarium testbed. 49 | """ 50 | print('##### DEBUG OUTPUT #####') 51 | print('Your simulation will take approximately {0} real seconds when deployed on the Robotarium. \n'.format(math.ceil(self._iterations*0.033))) 52 | 53 | if bool(self._errors): 54 | if "boundary" in self._errors: 55 | print('\t Simulation had {0} {1}\n'.format(self._errors["boundary"], self._errors["boundary_string"])) 56 | if "collision" in self._errors: 57 | print('\t Simulation had {0} {1}\n'.format(self._errors["collision"], self._errors["collision_string"])) 58 | if "actuator" in self._errors: 59 | print('\t Simulation had {0} {1}'.format(self._errors["actuator"], self._errors["actuator_string"])) 60 | else: 61 | print('No errors in your simulation! Acceptance of your experiment is likely!') 62 | 63 | return 64 | 65 | def step(self): 66 | """Increments the simulation by updating the dynamics. 67 | """ 68 | assert(not self._called_step_already), "Make sure to call get_poses before calling step() again." 69 | 70 | # Allow get_poses function to be called again. 71 | self._called_step_already = True 72 | self._checked_poses_already = False 73 | 74 | # Validate before thresholding velocities 75 | self._errors = self._validate() 76 | self._iterations += 1 77 | 78 | 79 | # Update dynamics of agents 80 | self.poses[0, :] = self.poses[0, :] + self.time_step*np.cos(self.poses[2,:])*self.velocities[0, :] 81 | self.poses[1, :] = self.poses[1, :] + self.time_step*np.sin(self.poses[2,:])*self.velocities[0, :] 82 | self.poses[2, :] = self.poses[2, :] + self.time_step*self.velocities[1, :] 83 | # Ensure angles are wrapped 84 | self.poses[2, :] = np.arctan2(np.sin(self.poses[2, :]), np.cos(self.poses[2, :])) 85 | 86 | # Update graphics 87 | if(self.show_figure): 88 | if(self.sim_in_real_time): 89 | t = time.time() 90 | while(t - self.previous_render_time < self.time_step): 91 | t=time.time() 92 | self.previous_render_time = t 93 | 94 | for i in range(self.number_of_robots): 95 | self.chassis_patches[i].center = self.poses[:2, i] 96 | self.chassis_patches[i].orientation = self.poses[2, i] + math.pi/4 97 | 98 | self.right_wheel_patches[i].center = self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]+math.pi/2), np.sin(self.poses[2, i]+math.pi/2)))+\ 99 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))) 100 | self.right_wheel_patches[i].orientation = self.poses[2, i] + math.pi/4 101 | 102 | self.left_wheel_patches[i].center = self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 103 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))) 104 | self.left_wheel_patches[i].orientation = self.poses[2,i] + math.pi/4 105 | 106 | self.right_led_patches[i].center = self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2,i]), np.sin(self.poses[2,i])))-\ 107 | 0.04*np.array((-np.sin(self.poses[2, i]), np.cos(self.poses[2, i]))) 108 | self.left_led_patches[i].center = self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2,i]), np.sin(self.poses[2,i])))-\ 109 | 0.015*np.array((-np.sin(self.poses[2, i]), np.cos(self.poses[2, i]))) 110 | 111 | self.figure.canvas.draw_idle() 112 | self.figure.canvas.flush_events() 113 | 114 | -------------------------------------------------------------------------------- /rps/robotarium.py.bak: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | import matplotlib.patches as patches 7 | from rps.robotarium_abc import * 8 | 9 | # Robotarium This object provides routines to interface with the Robotarium. 10 | # 11 | # THIS CLASS SHOULD NEVER BE MODIFIED OR SUBMITTED 12 | 13 | class Robotarium(RobotariumABC): 14 | 15 | def __init__(self, number_of_robots=-1, show_figure=True, sim_in_real_time = True, initial_conditions=np.array([])): 16 | super().__init__(number_of_robots, show_figure, sim_in_real_time, initial_conditions) 17 | 18 | #Initialize some rendering variables 19 | self.previous_render_time = time.time() 20 | self.sim_in_real_time = sim_in_real_time 21 | 22 | #Initialize checks for step and get poses calls 23 | self._called_step_already = True 24 | self._checked_poses_already = False 25 | 26 | #Initialization of error collection. 27 | self._errors = {} 28 | 29 | #Initialize steps 30 | self._iterations = 0 31 | 32 | def get_poses(self): 33 | """Returns the states of the agents. 34 | 35 | -> 3xN numpy array (of robot poses) 36 | """ 37 | 38 | assert(not self._checked_poses_already), "Can only call get_poses() once per call of step()." 39 | # Allow step() to be called again. 40 | self._called_step_already = False 41 | self._checked_poses_already = True 42 | 43 | return self.poses 44 | 45 | def call_at_scripts_end(self): 46 | """Call this function at the end of scripts to display potentail errors. 47 | Even if you don't want to print the errors, calling this function at the 48 | end of your script will enable execution on the Robotarium testbed. 49 | """ 50 | print('##### DEBUG OUTPUT #####') 51 | print('Your simulation will take approximately {0} real seconds when deployed on the Robotarium. \n'.format(math.ceil(self._iterations*0.033))) 52 | 53 | if bool(self._errors): 54 | if "boundary" in self._errors: 55 | print('\t Simulation had {0} {1}\n'.format(self._errors["boundary"], self._errors["boundary_string"])) 56 | if "collision" in self._errors: 57 | print('\t Simulation had {0} {1}\n'.format(self._errors["collision"], self._errors["collision_string"])) 58 | if "actuator" in self._errors: 59 | print('\t Simulation had {0} {1}'.format(self._errors["actuator"], self._errors["actuator_string"])) 60 | else: 61 | print('No errors in your simulation! Acceptance of your experiment is likely!') 62 | 63 | return 64 | 65 | def step(self): 66 | """Increments the simulation by updating the dynamics. 67 | """ 68 | assert(not self._called_step_already), "Make sure to call get_poses before calling step() again." 69 | 70 | # Allow get_poses function to be called again. 71 | self._called_step_already = True 72 | self._checked_poses_already = False 73 | 74 | # Validate before thresholding velocities 75 | self._errors = self._validate() 76 | self._iterations += 1 77 | 78 | 79 | # Update dynamics of agents 80 | self.poses[0, :] = self.poses[0, :] + self.time_step*np.cos(self.poses[2,:])*self.velocities[0, :] 81 | self.poses[1, :] = self.poses[1, :] + self.time_step*np.sin(self.poses[2,:])*self.velocities[0, :] 82 | self.poses[2, :] = self.poses[2, :] + self.time_step*self.velocities[1, :] 83 | # Ensure angles are wrapped 84 | self.poses[2, :] = np.arctan2(np.sin(self.poses[2, :]), np.cos(self.poses[2, :])) 85 | 86 | # Update graphics 87 | if(self.show_figure): 88 | if(self.sim_in_real_time): 89 | t = time.time() 90 | while(t - self.previous_render_time < self.time_step): 91 | t=time.time() 92 | self.previous_render_time = t 93 | 94 | for i in range(self.number_of_robots): 95 | self.chassis_patches[i].center = self.poses[:2, i] 96 | self.chassis_patches[i].orientation = self.poses[2, i] + math.pi/4 97 | 98 | self.right_wheel_patches[i].center = self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]+math.pi/2), np.sin(self.poses[2, i]+math.pi/2)))+\ 99 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))) 100 | self.right_wheel_patches[i].orientation = self.poses[2, i] + math.pi/4 101 | 102 | self.left_wheel_patches[i].center = self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 103 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))) 104 | self.left_wheel_patches[i].orientation = self.poses[2,i] + math.pi/4 105 | 106 | self.right_led_patches[i].center = self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2,i]), np.sin(self.poses[2,i])))-\ 107 | 0.04*np.array((-np.sin(self.poses[2, i]), np.cos(self.poses[2, i]))) 108 | self.left_led_patches[i].center = self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2,i]), np.sin(self.poses[2,i])))-\ 109 | 0.015*np.array((-np.sin(self.poses[2, i]), np.cos(self.poses[2, i]))) 110 | 111 | self.figure.canvas.draw_idle() 112 | self.figure.canvas.flush_events() 113 | 114 | -------------------------------------------------------------------------------- /rps/robotarium_abc.py: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | from abc import ABC, abstractmethod 4 | 5 | import numpy as np 6 | import matplotlib 7 | matplotlib.use('TkAgg') 8 | import matplotlib.pyplot as plt 9 | import matplotlib.patches as patches 10 | 11 | import rps.utilities.misc as misc 12 | 13 | # RobotariumABC: This is an interface for the Robotarium class that 14 | # ensures the simulator and the robots match up properly. 15 | 16 | # THIS FILE SHOULD NEVER BE MODIFIED OR SUBMITTED! 17 | 18 | class RobotariumABC(ABC): 19 | 20 | def __init__(self, number_of_robots=-1, show_figure=True, sim_in_real_time=True, initial_conditions=np.array([])): 21 | 22 | #Check user input types 23 | assert isinstance(number_of_robots,int), "The number of robots used argument (number_of_robots) provided to create the Robotarium object must be an integer type. Recieved type %r." % type(number_of_robots).__name__ 24 | assert isinstance(initial_conditions,np.ndarray), "The initial conditions array argument (initial_conditions) provided to create the Robotarium object must be a numpy ndarray. Recieved type %r." % type(initial_conditions).__name__ 25 | assert isinstance(show_figure,bool), "The display figure window argument (show_figure) provided to create the Robotarium object must be boolean type. Recieved type %r." % type(show_figure).__name__ 26 | assert isinstance(sim_in_real_time,bool), "The simulation running at 0.033s per loop (sim_real_time) provided to create the Robotarium object must be boolean type. Recieved type %r." % type(show_figure).__name__ 27 | 28 | #Check user input ranges/sizes 29 | assert (number_of_robots >= 0 and number_of_robots <= 50), "Requested %r robots to be used when creating the Robotarium object. The deployed number of robots must be between 0 and 50." % number_of_robots 30 | if (initial_conditions.size > 0): 31 | assert initial_conditions.shape == (3, number_of_robots), "Initial conditions provided when creating the Robotarium object must of size 3xN, where N is the number of robots used. Expected a 3 x %r array but recieved a %r x %r array." % (number_of_robots, initial_conditions.shape[0], initial_conditions.shape[1]) 32 | 33 | 34 | self.number_of_robots = number_of_robots 35 | self.show_figure = show_figure 36 | self.initial_conditions = initial_conditions 37 | 38 | # Boundary stuff -> lower left point / width / height 39 | self.boundaries = [-2, -2, 4, 4] 40 | 41 | self.file_path = None 42 | self.current_file_size = 0 43 | 44 | # Constants 45 | self.time_step = 0.1 46 | self.robot_diameter = 0.15 47 | self.wheel_radius = 0.016 48 | self.base_length = 0.105 49 | self.max_linear_velocity = 0.5 50 | self.max_angular_velocity = 2 51 | self.max_wheel_velocity = self.max_linear_velocity/self.wheel_radius 52 | 53 | self.robot_radius = self.robot_diameter/2 54 | 55 | self.velocities = np.zeros((2, number_of_robots)) 56 | self.poses = self.initial_conditions 57 | if self.initial_conditions.size == 0: 58 | self.poses = misc.generate_initial_conditions(self.number_of_robots, spacing=0.2, width=2.5, height=1.5) 59 | 60 | self.left_led_commands = [] 61 | self.right_led_commands = [] 62 | 63 | # Visualization 64 | self.figure = [] 65 | self.axes = [] 66 | self.left_led_patches = [] 67 | self.right_led_patches = [] 68 | self.chassis_patches = [] 69 | self.right_wheel_patches = [] 70 | self.left_wheel_patches = [] 71 | 72 | if(self.show_figure): 73 | self.figure, self.axes = plt.subplots() 74 | self.axes.set_axis_off() 75 | for i in range(number_of_robots): 76 | if i==0: 77 | p = patches.RegularPolygon(self.poses[:2, i], 4, math.sqrt(2)*self.robot_radius, self.poses[2,i]+math.pi/4, facecolor='#FFD700', edgecolor = 'k') 78 | if i>=1: 79 | p = patches.RegularPolygon(self.poses[:2, i], 4, math.sqrt(2)*self.robot_radius, self.poses[2,i]+math.pi/4, facecolor='#0028ff', edgecolor = 'k') 80 | 81 | rled = patches.Circle(self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2, i]), np.sin(self.poses[2, i]))+\ 82 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2)))),\ 83 | self.robot_radius/5, fill=False) 84 | lled = patches.Circle(self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2, i]), np.sin(self.poses[2, i]))+\ 85 | 0.015*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2)))),\ 86 | self.robot_radius/5, fill=False) 87 | rw = patches.Circle(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]+math.pi/2), np.sin(self.poses[2, i]+math.pi/2)))+\ 88 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))),\ 89 | 0.02, facecolor='k') 90 | lw = patches.Circle(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 91 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2))),\ 92 | 0.02, facecolor='k') 93 | #lw = patches.RegularPolygon(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 94 | # 0.035*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))),\ 95 | # 4, math.sqrt(2)*0.02, self.poses[2,i]+math.pi/4, facecolor='k') 96 | 97 | self.chassis_patches.append(p) 98 | self.left_led_patches.append(lled) 99 | self.right_led_patches.append(rled) 100 | self.right_wheel_patches.append(rw) 101 | self.left_wheel_patches.append(lw) 102 | 103 | self.axes.add_patch(rw) 104 | self.axes.add_patch(lw) 105 | self.axes.add_patch(p) 106 | self.axes.add_patch(lled) 107 | self.axes.add_patch(rled) 108 | 109 | # Draw arena 110 | self.boundary_patch = self.axes.add_patch(patches.Rectangle(self.boundaries[:2], self.boundaries[2], self.boundaries[3], fill=False)) 111 | self.boundary_patch2 = self.axes.add_patch(patches.Rectangle(np.array([0.5,0.5]), 1, 1, fill=False)) 112 | self.axes.set_xlim(self.boundaries[0]-0.1, self.boundaries[0]+self.boundaries[2]+0.1) 113 | self.axes.set_ylim(self.boundaries[1]-0.1, self.boundaries[1]+self.boundaries[3]+0.1) 114 | 115 | plt.ion() 116 | plt.show() 117 | 118 | plt.subplots_adjust(left=-0.03, right=1.03, bottom=-0.03, top=1.03, wspace=0, hspace=0) 119 | 120 | def set_velocities(self, ids, velocities): 121 | 122 | # Threshold linear velocities 123 | idxs = np.where(np.abs(velocities[0, :]) > self.max_linear_velocity) 124 | velocities[0, idxs] = self.max_linear_velocity*np.sign(velocities[0, idxs]) 125 | 126 | # Threshold angular velocities 127 | idxs = np.where(np.abs(velocities[1, :]) > self.max_angular_velocity) 128 | velocities[1, idxs] = self.max_angular_velocity*np.sign(velocities[1, idxs]) 129 | self.velocities = velocities 130 | 131 | def reset(self , poses): 132 | 133 | self.poses = poses 134 | 135 | def set_poses(self, poses): 136 | 137 | self.poses = poses 138 | self.velocities = np.zeros((2, 2)) 139 | 140 | # def set_velocities(self, ids, velocities): 141 | 142 | # # Threshold linear velocities 143 | # idxs = np.where(np.abs(velocities[0, :]) > self.max_linear_velocity) 144 | # velocities[0, idxs] = self.max_linear_velocity*np.sign(velocities[0, idxs]) 145 | 146 | # # Threshold angular velocities 147 | # idxs = np.where(np.abs(velocities[1, :]) > self.max_angular_velocity) 148 | # velocities[1, idxs] = self.max_angular_velocity*np.sign(velocities[1, idxs]) 149 | # self.velocities = velocities 150 | 151 | @abstractmethod 152 | def get_poses(self): 153 | raise NotImplementedError() 154 | 155 | @abstractmethod 156 | def step(self): 157 | raise NotImplementedError() 158 | 159 | #Protected Functions 160 | def _threshold(self, dxu): 161 | dxdd = self._uni_to_diff(dxu) 162 | 163 | to_thresh = np.absolute(dxdd) > self.max_wheel_velocity 164 | dxdd[to_thresh] = self.max_wheel_velocity*np.sign(dxdd[to_thresh]) 165 | 166 | dxu = self._diff_to_uni(dxdd) 167 | 168 | def _uni_to_diff(self, dxu): 169 | r = self.wheel_radius 170 | l = self.base_length 171 | dxdd = np.vstack((1/(2*r)*(2*dxu[0,:]-l*dxu[1,:]),1/(2*r)*(2*dxu[0,:]+l*dxu[1,:]))) 172 | 173 | return dxdd 174 | 175 | def _diff_to_uni(self, dxdd): 176 | r = self.wheel_radius 177 | l = self.base_length 178 | dxu = np.vstack((r/(2)*(dxdd[0,:]+dxdd[1,:]),r/l*(dxdd[1,:]-dxdd[0,:]))) 179 | 180 | return dxu 181 | 182 | def _validate(self, errors = {}): 183 | # This is meant to be called on every iteration of step. 184 | # Checks to make sure robots are operating within the bounds of reality. 185 | 186 | p = self.poses 187 | b = self.boundaries 188 | N = self.number_of_robots 189 | 190 | 191 | for i in range(N): 192 | x = p[0,i] 193 | y = p[1,i] 194 | 195 | if(x < b[0] or x > (b[0] + b[2]) or y < b[1] or y > (b[1] + b[3])): 196 | if "boundary" in errors: 197 | errors["boundary"] += 1 198 | else: 199 | errors["boundary"] = 1 200 | errors["boundary_string"] = "iteration(s) robots were outside the boundaries." 201 | 202 | for j in range(N-1): 203 | for k in range(j+1,N): 204 | if(np.linalg.norm(p[:2,j]-p[:2,k]) <= self.robot_diameter): 205 | if "collision" in errors: 206 | errors["collision"] += 1 207 | else: 208 | errors["collision"] = 1 209 | errors["collision_string"] = "iteration(s) where robots collided." 210 | 211 | dxdd = self._uni_to_diff(self.velocities) 212 | exceeding = np.absolute(dxdd) > self.max_wheel_velocity 213 | if(np.any(exceeding)): 214 | if "actuator" in errors: 215 | errors["actuator"] += 1 216 | else: 217 | errors["actuator"] = 1 218 | errors["actuator_string"] = "iteration(s) where the actuator limits were exceeded." 219 | 220 | return errors 221 | 222 | -------------------------------------------------------------------------------- /rps/robotarium_abc.py.bak: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | from abc import ABC, abstractmethod 4 | 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | import matplotlib.patches as patches 8 | 9 | import rps.utilities.misc as misc 10 | 11 | # RobotariumABC: This is an interface for the Robotarium class that 12 | # ensures the simulator and the robots match up properly. 13 | 14 | # THIS FILE SHOULD NEVER BE MODIFIED OR SUBMITTED! 15 | 16 | class RobotariumABC(ABC): 17 | 18 | def __init__(self, number_of_robots=-1, show_figure=True, sim_in_real_time=True, initial_conditions=np.array([])): 19 | 20 | #Check user input types 21 | assert isinstance(number_of_robots,int), "The number of robots used argument (number_of_robots) provided to create the Robotarium object must be an integer type. Recieved type %r." % type(number_of_robots).__name__ 22 | assert isinstance(initial_conditions,np.ndarray), "The initial conditions array argument (initial_conditions) provided to create the Robotarium object must be a numpy ndarray. Recieved type %r." % type(initial_conditions).__name__ 23 | assert isinstance(show_figure,bool), "The display figure window argument (show_figure) provided to create the Robotarium object must be boolean type. Recieved type %r." % type(show_figure).__name__ 24 | assert isinstance(sim_in_real_time,bool), "The simulation running at 0.033s per loop (sim_real_time) provided to create the Robotarium object must be boolean type. Recieved type %r." % type(show_figure).__name__ 25 | 26 | #Check user input ranges/sizes 27 | assert (number_of_robots >= 0 and number_of_robots <= 50), "Requested %r robots to be used when creating the Robotarium object. The deployed number of robots must be between 0 and 50." % number_of_robots 28 | if (initial_conditions.size > 0): 29 | assert initial_conditions.shape == (3, number_of_robots), "Initial conditions provided when creating the Robotarium object must of size 3xN, where N is the number of robots used. Expected a 3 x %r array but recieved a %r x %r array." % (number_of_robots, initial_conditions.shape[0], initial_conditions.shape[1]) 30 | 31 | 32 | self.number_of_robots = number_of_robots 33 | self.show_figure = show_figure 34 | self.initial_conditions = initial_conditions 35 | 36 | # Boundary stuff -> lower left point / width / height 37 | self.boundaries = [-1.5, -1.5, 3, 3] 38 | 39 | self.file_path = None 40 | self.current_file_size = 0 41 | 42 | # Constants 43 | self.time_step = 0.05 44 | self.robot_diameter = 0.15 45 | self.wheel_radius = 0.016 46 | self.base_length = 0.105 47 | self.max_linear_velocity = 0.5 48 | self.max_angular_velocity = 2 49 | self.max_wheel_velocity = self.max_linear_velocity/self.wheel_radius 50 | 51 | self.robot_radius = self.robot_diameter/2 52 | 53 | self.velocities = np.zeros((2, number_of_robots)) 54 | self.poses = self.initial_conditions 55 | if self.initial_conditions.size == 0: 56 | self.poses = misc.generate_initial_conditions(self.number_of_robots, spacing=0.2, width=2.5, height=1.5) 57 | 58 | self.left_led_commands = [] 59 | self.right_led_commands = [] 60 | 61 | # Visualization 62 | self.figure = [] 63 | self.axes = [] 64 | self.left_led_patches = [] 65 | self.right_led_patches = [] 66 | self.chassis_patches = [] 67 | self.right_wheel_patches = [] 68 | self.left_wheel_patches = [] 69 | 70 | if(self.show_figure): 71 | self.figure, self.axes = plt.subplots() 72 | self.axes.set_axis_off() 73 | for i in range(number_of_robots): 74 | if i==0: 75 | p = patches.RegularPolygon(self.poses[:2, i], 4, math.sqrt(2)*self.robot_radius, self.poses[2,i]+math.pi/4, facecolor='#FFD700', edgecolor = 'k') 76 | if i==1: 77 | p = patches.RegularPolygon(self.poses[:2, i], 4, math.sqrt(2)*self.robot_radius, self.poses[2,i]+math.pi/4, facecolor='#0028ff', edgecolor = 'k') 78 | 79 | rled = patches.Circle(self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2, i]), np.sin(self.poses[2, i]))+\ 80 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2)))),\ 81 | self.robot_radius/5, fill=False) 82 | lled = patches.Circle(self.poses[:2, i]+0.75*self.robot_radius*np.array((np.cos(self.poses[2, i]), np.sin(self.poses[2, i]))+\ 83 | 0.015*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2)))),\ 84 | self.robot_radius/5, fill=False) 85 | rw = patches.Circle(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]+math.pi/2), np.sin(self.poses[2, i]+math.pi/2)))+\ 86 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))),\ 87 | 0.02, facecolor='k') 88 | lw = patches.Circle(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 89 | 0.04*np.array((-np.sin(self.poses[2, i]+math.pi/2))),\ 90 | 0.02, facecolor='k') 91 | #lw = patches.RegularPolygon(self.poses[:2, i]+self.robot_radius*np.array((np.cos(self.poses[2, i]-math.pi/2), np.sin(self.poses[2, i]-math.pi/2)))+\ 92 | # 0.035*np.array((-np.sin(self.poses[2, i]+math.pi/2), np.cos(self.poses[2, i]+math.pi/2))),\ 93 | # 4, math.sqrt(2)*0.02, self.poses[2,i]+math.pi/4, facecolor='k') 94 | 95 | self.chassis_patches.append(p) 96 | self.left_led_patches.append(lled) 97 | self.right_led_patches.append(rled) 98 | self.right_wheel_patches.append(rw) 99 | self.left_wheel_patches.append(lw) 100 | 101 | self.axes.add_patch(rw) 102 | self.axes.add_patch(lw) 103 | self.axes.add_patch(p) 104 | self.axes.add_patch(lled) 105 | self.axes.add_patch(rled) 106 | 107 | # Draw arena 108 | self.boundary_patch = self.axes.add_patch(patches.Rectangle(self.boundaries[:2], self.boundaries[2], self.boundaries[3], fill=False)) 109 | self.boundary_patch2 = self.axes.add_patch(patches.Rectangle(np.array([0.5,0.5]), 1, 1, fill=False)) 110 | self.axes.set_xlim(self.boundaries[0]-0.1, self.boundaries[0]+self.boundaries[2]+0.1) 111 | self.axes.set_ylim(self.boundaries[1]-0.1, self.boundaries[1]+self.boundaries[3]+0.1) 112 | 113 | plt.ion() 114 | plt.show() 115 | 116 | plt.subplots_adjust(left=-0.03, right=1.03, bottom=-0.03, top=1.03, wspace=0, hspace=0) 117 | 118 | def set_velocities(self, ids, velocities): 119 | 120 | # Threshold linear velocities 121 | idxs = np.where(np.abs(velocities[0, :]) > self.max_linear_velocity) 122 | velocities[0, idxs] = self.max_linear_velocity*np.sign(velocities[0, idxs]) 123 | 124 | # Threshold angular velocities 125 | idxs = np.where(np.abs(velocities[1, :]) > self.max_angular_velocity) 126 | velocities[1, idxs] = self.max_angular_velocity*np.sign(velocities[1, idxs]) 127 | self.velocities = velocities 128 | 129 | def reset(self , poses): 130 | 131 | self.poses = poses 132 | 133 | def set_poses(self, poses): 134 | 135 | self.poses = poses 136 | self.velocities = np.zeros((2, 2)) 137 | 138 | # def set_velocities(self, ids, velocities): 139 | 140 | # # Threshold linear velocities 141 | # idxs = np.where(np.abs(velocities[0, :]) > self.max_linear_velocity) 142 | # velocities[0, idxs] = self.max_linear_velocity*np.sign(velocities[0, idxs]) 143 | 144 | # # Threshold angular velocities 145 | # idxs = np.where(np.abs(velocities[1, :]) > self.max_angular_velocity) 146 | # velocities[1, idxs] = self.max_angular_velocity*np.sign(velocities[1, idxs]) 147 | # self.velocities = velocities 148 | 149 | @abstractmethod 150 | def get_poses(self): 151 | raise NotImplementedError() 152 | 153 | @abstractmethod 154 | def step(self): 155 | raise NotImplementedError() 156 | 157 | #Protected Functions 158 | def _threshold(self, dxu): 159 | dxdd = self._uni_to_diff(dxu) 160 | 161 | to_thresh = np.absolute(dxdd) > self.max_wheel_velocity 162 | dxdd[to_thresh] = self.max_wheel_velocity*np.sign(dxdd[to_thresh]) 163 | 164 | dxu = self._diff_to_uni(dxdd) 165 | 166 | def _uni_to_diff(self, dxu): 167 | r = self.wheel_radius 168 | l = self.base_length 169 | dxdd = np.vstack((1/(2*r)*(2*dxu[0,:]-l*dxu[1,:]),1/(2*r)*(2*dxu[0,:]+l*dxu[1,:]))) 170 | 171 | return dxdd 172 | 173 | def _diff_to_uni(self, dxdd): 174 | r = self.wheel_radius 175 | l = self.base_length 176 | dxu = np.vstack((r/(2)*(dxdd[0,:]+dxdd[1,:]),r/l*(dxdd[1,:]-dxdd[0,:]))) 177 | 178 | return dxu 179 | 180 | def _validate(self, errors = {}): 181 | # This is meant to be called on every iteration of step. 182 | # Checks to make sure robots are operating within the bounds of reality. 183 | 184 | p = self.poses 185 | b = self.boundaries 186 | N = self.number_of_robots 187 | 188 | 189 | for i in range(N): 190 | x = p[0,i] 191 | y = p[1,i] 192 | 193 | if(x < b[0] or x > (b[0] + b[2]) or y < b[1] or y > (b[1] + b[3])): 194 | if "boundary" in errors: 195 | errors["boundary"] += 1 196 | else: 197 | errors["boundary"] = 1 198 | errors["boundary_string"] = "iteration(s) robots were outside the boundaries." 199 | 200 | for j in range(N-1): 201 | for k in range(j+1,N): 202 | if(np.linalg.norm(p[:2,j]-p[:2,k]) <= self.robot_diameter): 203 | if "collision" in errors: 204 | errors["collision"] += 1 205 | else: 206 | errors["collision"] = 1 207 | errors["collision_string"] = "iteration(s) where robots collided." 208 | 209 | dxdd = self._uni_to_diff(self.velocities) 210 | exceeding = np.absolute(dxdd) > self.max_wheel_velocity 211 | if(np.any(exceeding)): 212 | if "actuator" in errors: 213 | errors["actuator"] += 1 214 | else: 215 | errors["actuator"] = 1 216 | errors["actuator_string"] = "iteration(s) where the actuator limits were exceeded." 217 | 218 | return errors 219 | 220 | -------------------------------------------------------------------------------- /rps/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EdmundLuan/CLF_CBF_NMPC_python/9e89e585b67055f31e9324815a5267ed4c29894d/rps/utilities/__init__.py -------------------------------------------------------------------------------- /rps/utilities/barrier_certificates.py: -------------------------------------------------------------------------------- 1 | from cvxopt import matrix 2 | from cvxopt.blas import dot 3 | from cvxopt.solvers import qp, options 4 | from cvxopt import matrix, sparse 5 | 6 | # Unused for now, will include later for speed. 7 | # import quadprog as solver2 8 | 9 | import itertools 10 | import numpy as np 11 | from scipy.special import comb 12 | 13 | from rps.utilities.transformations import * 14 | 15 | # Disable output of CVXOPT 16 | options['show_progress'] = False 17 | # Change default options of CVXOPT for faster solving 18 | options['reltol'] = 1e-2 # was e-2 19 | options['feastol'] = 1e-2 # was e-4 20 | options['maxiters'] = 50 # default is 100 21 | 22 | def create_single_integrator_barrier_certificate(barrier_gain=100, safety_radius=0.17, magnitude_limit=0.2): 23 | """Creates a barrier certificate for a single-integrator system. This function 24 | returns another function for optimization reasons. 25 | 26 | barrier_gain: double (controls how quickly agents can approach each other. lower = slower) 27 | safety_radius: double (how far apart the agents will stay) 28 | magnitude_limit: how fast the robot can move linearly. 29 | 30 | -> function (the barrier certificate function) 31 | """ 32 | 33 | #Check user input types 34 | assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 35 | assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 36 | assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 37 | 38 | #Check user input ranges/sizes 39 | assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain 40 | 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 41 | 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 42 | 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 43 | 44 | 45 | def f(dxi, x): 46 | #Check user input types 47 | assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 48 | assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 49 | 50 | #Check user input ranges/sizes 51 | assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] 52 | assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 53 | assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) 54 | 55 | 56 | # Initialize some variables for computational savings 57 | N = dxi.shape[1] 58 | num_constraints = int(comb(N, 2)) 59 | A = np.zeros((num_constraints, 2*N)) 60 | b = np.zeros(num_constraints) 61 | H = sparse(matrix(2*np.identity(2*N))) 62 | 63 | count = 0 64 | for i in range(N-1): 65 | for j in range(i+1, N): 66 | error = x[:, i] - x[:, j] 67 | h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) 68 | 69 | A[count, (2*i, (2*i+1))] = -2*error 70 | A[count, (2*j, (2*j+1))] = 2*error 71 | b[count] = barrier_gain*np.power(h, 3) 72 | 73 | count += 1 74 | 75 | # Threshold control inputs before QP 76 | norms = np.linalg.norm(dxi, 2, 0) 77 | idxs_to_normalize = (norms > magnitude_limit) 78 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 79 | 80 | f = -2*np.reshape(dxi, 2*N, order='F') 81 | result = qp(H, matrix(f), matrix(A), matrix(b))['x'] 82 | 83 | return np.reshape(result, (2, -1), order='F') 84 | 85 | return f 86 | 87 | def create_single_integrator_barrier_certificate_with_boundary(barrier_gain=100, safety_radius=0.17, magnitude_limit=0.2, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): 88 | """Creates a barrier certificate for a single-integrator system with a rectangular boundary included. This function 89 | returns another function for optimization reasons. 90 | 91 | barrier_gain: double (controls how quickly agents can approach each other. lower = slower) 92 | safety_radius: double (how far apart the agents will stay) 93 | magnitude_limit: how fast the robot can move linearly. 94 | 95 | -> function (the barrier certificate function) 96 | """ 97 | 98 | #Check user input types 99 | assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 100 | assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 101 | assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 102 | 103 | #Check user input ranges/sizes 104 | assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain 105 | 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 106 | 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 107 | 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 108 | 109 | 110 | def f(dxi, x): 111 | #Check user input types 112 | assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 113 | assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 114 | 115 | #Check user input ranges/sizes 116 | assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] 117 | assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 118 | assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) 119 | 120 | 121 | # Initialize some variables for computational savings 122 | N = dxi.shape[1] 123 | num_constraints = int(comb(N, 2)) + 4*N 124 | A = np.zeros((num_constraints, 2*N)) 125 | b = np.zeros(num_constraints) 126 | #H = sparse(matrix(2*np.identity(2*N))) 127 | H = 2*np.identity(2*N) 128 | 129 | count = 0 130 | for i in range(N-1): 131 | for j in range(i+1, N): 132 | error = x[:, i] - x[:, j] 133 | h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) 134 | 135 | A[count, (2*i, (2*i+1))] = -2*error 136 | A[count, (2*j, (2*j+1))] = 2*error 137 | b[count] = barrier_gain*np.power(h, 3) 138 | 139 | count += 1 140 | 141 | for k in range(N): 142 | #Pos Y 143 | A[count, (2*k, 2*k+1)] = np.array([0,1]) 144 | b[count] = 0.4*barrier_gain*(boundary_points[3] - safety_radius/2 - x[1,k])**3; 145 | count += 1 146 | 147 | #Neg Y 148 | A[count, (2*k, 2*k+1)] = -np.array([0,1]) 149 | b[count] = 0.4*barrier_gain*(-boundary_points[2] - safety_radius/2 + x[1,k])**3; 150 | count += 1 151 | 152 | #Pos X 153 | A[count, (2*k, 2*k+1)] = np.array([1,0]) 154 | b[count] = 0.4*barrier_gain*(boundary_points[1] - safety_radius/2 - x[0,k])**3; 155 | count += 1 156 | 157 | #Neg X 158 | A[count, (2*k, 2*k+1)] = -np.array([1,0]) 159 | b[count] = 0.4*barrier_gain*(-boundary_points[0] - safety_radius/2 + x[0,k])**3; 160 | count += 1 161 | 162 | # Threshold control inputs before QP 163 | norms = np.linalg.norm(dxi, 2, 0) 164 | idxs_to_normalize = (norms > magnitude_limit) 165 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 166 | 167 | f = -2*np.reshape(dxi, (2*N,1), order='F') 168 | b = np.reshape(b, (count,1), order='F') 169 | result = qp(matrix(H), matrix(f), matrix(A), matrix(b))['x'] 170 | #result = solver2.solve_qp(H, f, A, b, 0)[0] 171 | 172 | return np.reshape(result, (2, N), order='F') 173 | 174 | return f 175 | 176 | def create_single_integrator_barrier_certificate2(barrier_gain=100, unsafe_barrier_gain=1e6, safety_radius=0.17, magnitude_limit=0.2): 177 | """Creates a barrier certificate for a single-integrator system. This function 178 | returns another function for optimization reasons. This function is different from 179 | create_single_integrator_barrier_certificate as it changes the barrier gain to a large 180 | number if the single integrator point enters the unsafe region. 181 | 182 | barrier_gain: double (controls how quickly agents can approach each other. lower = slower) 183 | safety_radius: double (how far apart the agents will stay) 184 | magnitude_limit: how fast the robot can move linearly. 185 | 186 | -> function (the barrier certificate function) 187 | """ 188 | 189 | #Check user input types 190 | assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 191 | assert isinstance(unsafe_barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate2, the barrier gain if outside the safe set (unsafe_barrier_gain) must be an integer or float. Recieved type %r." % type(unsafe_barrier_gain).__name__ 192 | assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate2, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 193 | assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 194 | 195 | #Check user input ranges/sizes 196 | assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be positive. Recieved %r." % barrier_gain 197 | assert unsafe_barrier_gain > 0, "In the function create_single_integrator_barrier_certificate2, the barrier gain if outside the safe set (unsafe_barrier_gain) must be positive. Recieved %r." % unsafe_barrier_gain 198 | assert safety_radius >= 0.12, "In the function create_single_integrator_barrier_certificate2, 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 199 | assert magnitude_limit > 0, "In the function create_single_integrator_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit 200 | assert magnitude_limit <= 0.2, "In the function create_single_integrator_barrier_certificate2, 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 201 | 202 | 203 | def f(dxi, x): 204 | #Check user input types 205 | assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate2 function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 206 | assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate2 function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 207 | 208 | #Check user input ranges/sizes 209 | assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate2 function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] 210 | assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate2 function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 211 | assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate2 function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) 212 | 213 | 214 | # Initialize some variables for computational savings 215 | N = dxi.shape[1] 216 | num_constraints = int(comb(N, 2)) 217 | A = np.zeros((num_constraints, 2*N)) 218 | b = np.zeros(num_constraints) 219 | H = sparse(matrix(2*np.identity(2*N))) 220 | 221 | count = 0 222 | for i in range(N-1): 223 | for j in range(i+1, N): 224 | error = x[:, i] - x[:, j] 225 | h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) 226 | 227 | A[count, (2*i, (2*i+1))] = -2*error 228 | A[count, (2*j, (2*j+1))] = 2*error 229 | if h >= 0: 230 | b[count] = barrier_gain*np.power(h, 3) 231 | else: 232 | b[count] = unsafe_barrier_gain*np.power(h, 3) 233 | 234 | count += 1 235 | 236 | # Threshold control inputs before QP 237 | norms = np.linalg.norm(dxi, 2, 0) 238 | idxs_to_normalize = (norms > magnitude_limit) 239 | dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] 240 | 241 | f = -2*np.reshape(dxi, 2*N, order='F') 242 | result = qp(H, matrix(f), matrix(A), matrix(b))['x'] 243 | 244 | return np.reshape(result, (2, -1), order='F') 245 | 246 | return f 247 | 248 | def create_unicycle_barrier_certificate(barrier_gain=100, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2): 249 | """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping 250 | and single integrator implementation. For optimization purposes, this function returns 251 | another function. 252 | 253 | barrier_gain: double (how fast the robots can approach each other) 254 | safety_radius: double (how far apart the robots should stay) 255 | projection_distance: double (how far ahead to place the bubble) 256 | 257 | -> function (the unicycle barrier certificate function) 258 | """ 259 | 260 | #Check user input types 261 | assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 262 | assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 263 | assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ 264 | assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 265 | 266 | #Check user input ranges/sizes 267 | assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain 268 | assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius 269 | assert projection_distance > 0, "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance 270 | assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit 271 | assert magnitude_limit <= 0.2, "In the function create_unicycle_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 272 | 273 | 274 | si_barrier_cert = create_single_integrator_barrier_certificate(barrier_gain=barrier_gain, safety_radius=safety_radius+projection_distance) 275 | 276 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) 277 | 278 | uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) 279 | 280 | def f(dxu, x): 281 | #Check user input types 282 | assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ 283 | assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 284 | 285 | #Check user input ranges/sizes 286 | assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] 287 | assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] 288 | assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) 289 | 290 | 291 | x_si = uni_to_si_states(x) 292 | #Convert unicycle control command to single integrator one 293 | dxi = uni_to_si_dyn(dxu, x) 294 | #Apply single integrator barrier certificate 295 | dxi = si_barrier_cert(dxi, x_si) 296 | #Return safe unicycle command 297 | return si_to_uni_dyn(dxi, x) 298 | 299 | return f 300 | 301 | def create_unicycle_barrier_certificate_with_boundary(barrier_gain=100, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): 302 | """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping 303 | and single integrator implementation. For optimization purposes, this function returns 304 | another function. 305 | 306 | barrier_gain: double (how fast the robots can approach each other) 307 | safety_radius: double (how far apart the robots should stay) 308 | projection_distance: double (how far ahead to place the bubble) 309 | 310 | -> function (the unicycle barrier certificate function) 311 | """ 312 | 313 | #Check user input types 314 | assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 315 | assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 316 | assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ 317 | assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 318 | 319 | #Check user input ranges/sizes 320 | assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain 321 | assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius 322 | assert projection_distance > 0, "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance 323 | assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit 324 | assert magnitude_limit <= 0.2, "In the function create_unicycle_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 325 | 326 | 327 | si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary(barrier_gain=barrier_gain, safety_radius=safety_radius+projection_distance, boundary_points=boundary_points) 328 | 329 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) 330 | 331 | uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) 332 | 333 | def f(dxu, x): 334 | #Check user input types 335 | assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ 336 | assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 337 | 338 | #Check user input ranges/sizes 339 | assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] 340 | assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] 341 | assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) 342 | 343 | 344 | x_si = uni_to_si_states(x) 345 | #Convert unicycle control command to single integrator one 346 | dxi = uni_to_si_dyn(dxu, x) 347 | #Apply single integrator barrier certificate 348 | dxi = si_barrier_cert(dxi, x_si) 349 | #Return safe unicycle command 350 | return si_to_uni_dyn(dxi, x) 351 | 352 | return f 353 | 354 | def create_unicycle_barrier_certificate2(barrier_gain=500, unsafe_barrier_gain=1e6, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2): 355 | """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping 356 | and single integrator implementation. For optimization purposes, this function returns 357 | another function. 358 | 359 | barrier_gain: double (how fast the robots can approach each other) 360 | safety_radius: double (how far apart the robots should stay) 361 | projection_distance: double (how far ahead to place the bubble) 362 | 363 | -> function (the unicycle barrier certificate function) 364 | """ 365 | 366 | #Check user input types 367 | assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ 368 | assert isinstance(unsafe_barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate2, the barrier gain outside the safe set (unsafe_barrier_gain) must be an integer or float. Recieved type %r." % type(unsafe_barrier_gain).__name__ 369 | assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate2, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ 370 | assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate2, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ 371 | assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ 372 | 373 | #Check user input ranges/sizes 374 | assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be positive. Recieved %r." % barrier_gain 375 | assert unsafe_barrier_gain > 0, "In the function create_unicycle_barrier_certificate2, the barrier gain outside the safe set (unsafe_barrier_gain) must be positive. Recieved %r." % unsafe_barrier_gain 376 | assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate2, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius 377 | assert projection_distance > 0, "In the function create_unicycle_barrier_certificate2, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance 378 | assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit 379 | assert magnitude_limit <= 0.2, "In the function create_unicycle_barrier_certificate2, 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 380 | 381 | 382 | si_barrier_cert = create_single_integrator_barrier_certificate2(barrier_gain=barrier_gain, unsafe_barrier_gain=unsafe_barrier_gain, safety_radius=safety_radius+projection_distance) 383 | 384 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) 385 | 386 | uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) 387 | 388 | def f(dxu, x): 389 | #Check user input types 390 | assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ 391 | assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ 392 | 393 | #Check user input ranges/sizes 394 | assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] 395 | assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] 396 | assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) 397 | 398 | 399 | x_si = uni_to_si_states(x) 400 | #Convert unicycle control command to single integrator one 401 | dxi = uni_to_si_dyn(dxu, x) 402 | #Apply single integrator barrier certificate 403 | dxi = si_barrier_cert(dxi, x_si) 404 | #Return safe unicycle command 405 | return si_to_uni_dyn(dxi, x) 406 | 407 | return f 408 | 409 | def create_unicycle_differential_drive_barrier_certificate(max_num_obstacle_points = 100, max_num_robots = 30, disturbance = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, 410 | projection_distance =0.05, barrier_gain = 150, safety_radius = 0.17): 411 | 412 | 413 | D = np.matrix([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) 414 | L = np.matrix([[1,0],[0,projection_distance]])* D 415 | disturb = np.matrix([[-disturbance, -disturbance, disturbance, disturbance],[-disturbance, disturbance, disturbance, -disturbance]]) 416 | num_disturbs = np.size(disturb[1,:]) 417 | 418 | max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacle_points 419 | A = np.matrix(np.zeros([max_num_constraints, 2*max_num_robots])) 420 | b = np.matrix(np.zeros([max_num_constraints, 1])) 421 | Os = np.matrix(np.zeros([2,max_num_robots])) 422 | ps = np.matrix(np.zeros([2,max_num_robots])) 423 | Ms = np.matrix(np.zeros([2,2*max_num_robots])) 424 | 425 | def robust_barriers(dxu, x, obstacles=np.empty(0)): 426 | 427 | num_robots = np.size(dxu[0,:]) 428 | 429 | if obstacles.size != 0: 430 | num_obstacles = np.size(obstacles[0,:]) 431 | else: 432 | num_obstacles = 0 433 | 434 | if(num_robots < 2): 435 | temp = 0 436 | else: 437 | temp = (num_robots**2-num_robots)//2 438 | 439 | 440 | # Generate constraints for barrier certificates based on the size of the safety radius 441 | num_constraints = temp + num_robots*num_obstacles 442 | A[0:num_constraints, 0:2*num_robots] = 0 443 | Os[0, 0:num_robots] = np.cos(x[2, :]) 444 | Os[1, 0:num_robots] = np.sin(x[2, :]) 445 | ps[:, 0:num_robots] = x[0:2, :] + projection_distance*Os[:, 0:num_robots] 446 | # Ms Real Form 447 | # Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] 448 | # Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] 449 | # Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] 450 | # Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] 451 | # Flipped Ms to be able to perform desired matrix multiplication 452 | Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] 453 | Ms[0, 1:2*num_robots:2] = Os[1, 0:num_robots] 454 | Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] 455 | Ms[1, 0:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] 456 | MDs = (Ms.T * D).T 457 | temp = np.copy(MDs[1, 0:2*num_robots:2]) 458 | MDs[1, 0:2*num_robots:2] = MDs[0, 1:2*num_robots:2] 459 | MDs[0, 1:2*num_robots:2] = temp 460 | 461 | count = 0 462 | 463 | for i in range(num_robots-1): 464 | diffs = ps[:,i] - ps[:, i+1:num_robots] 465 | hs = np.sum(np.square(diffs),0) - safety_radius**2 # 1 by N 466 | h_dot_is = 2*diffs.T*MDs[:,(2*i, 2*i+1)] # N by 2 467 | h_dot_js = np.matrix(np.zeros((2,num_robots - (i+1)))) 468 | h_dot_js[0, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1):2*num_robots:2]), 0) 469 | h_dot_js[1, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1)+1:2*num_robots:2]), 0) 470 | new_constraints = num_robots - i - 1 471 | A[count:count+new_constraints, (2*i):(2*i+2)] = h_dot_is 472 | A[range(count,count+new_constraints), range(2*(i+1),2*num_robots,2)] = h_dot_js[0,:] 473 | A[range(count,count+new_constraints), range(2*(i+1)+1,2*num_robots,2)] = h_dot_js[1,:] 474 | b[count:count+new_constraints] = -barrier_gain*(np.power(hs,3)).T - np.min(h_dot_is*disturb,1) - np.min(h_dot_js.T*disturb,1) 475 | count += new_constraints 476 | 477 | if obstacles.size != 0: 478 | # Do obstacles 479 | for i in range(num_robots): 480 | diffs = (ps[:, i] - obstacles) 481 | h = np.sum(np.square(diffs),0) - safety_radius**2 482 | h_dot_i = 2*diffs.T*MDs[:,2*i:2*i+2] 483 | A[count:count+num_obstacles,(2*i):(2*i+2)] = h_dot_i 484 | b[count:count+num_obstacles] = -barrier_gain*(np.power(h,3)).T - np.min(h_dot_i*disturb, 1) 485 | count = count + num_obstacles 486 | 487 | # Adding Upper Bounds On Wheels 488 | A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) 489 | b[count:count+2*num_robots] = -wheel_vel_limit 490 | count += 2*num_robots 491 | # # Adding Lower Bounds on Wheels 492 | A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) 493 | b[count:count+2*num_robots] = -wheel_vel_limit 494 | count += 2*num_robots 495 | 496 | # Solve QP program generated earlier 497 | L_all = np.kron(np.eye(num_robots), L) 498 | dxu = np.linalg.inv(D)*dxu # Convert user input to differential drive 499 | vhat = np.matrix(np.reshape(dxu ,(2*num_robots,1), order='F')) 500 | H = 2*L_all.T*L_all 501 | f = np.transpose(-2*np.transpose(vhat)*np.transpose(L_all)*L_all) 502 | 503 | # Alternative Solver 504 | #start = time.time() 505 | #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 506 | #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) 507 | 508 | 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] 509 | # Initial Guess for Solver at the Next Iteration 510 | # 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); 511 | # Set robot velocities to new velocities 512 | dxu = np.reshape(vnew, (2, num_robots), order='F') 513 | dxu = D*dxu 514 | 515 | return dxu 516 | 517 | return robust_barriers 518 | 519 | def create_unicycle_differential_drive_barrier_certificate_with_boundary(max_num_obstacle_points = 100, max_num_robots = 30, disturbance = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, 520 | projection_distance =0.05, barrier_gain = 150, safety_radius = 0.17, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): 521 | 522 | 523 | D = np.array([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) 524 | L = np.array([[1,0],[0,projection_distance]]).dot(D) 525 | disturb = np.array([[-disturbance, -disturbance, disturbance, disturbance],[-disturbance, disturbance, disturbance, -disturbance]]) 526 | num_disturbs = disturb.shape[1] 527 | 528 | max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacle_points 529 | A = np.zeros([max_num_constraints, 2*max_num_robots]) 530 | b = np.zeros([max_num_constraints, 1]) 531 | Os = np.zeros([2,max_num_robots]) 532 | ps = np.zeros([2,max_num_robots]) 533 | Ms = np.zeros([2,2*max_num_robots]) 534 | 535 | def robust_barriers(dxu, x, obstacles=np.empty(0)): 536 | 537 | num_robots = np.size(dxu[0,:]) 538 | 539 | if obstacles.size != 0: 540 | num_obstacles = np.size(obstacles[0,:]) 541 | else: 542 | num_obstacles = 0 543 | 544 | if(num_robots < 2): 545 | temp = 0 546 | else: 547 | temp = (num_robots**2-num_robots)//2 548 | 549 | 550 | # Generate constraints for barrier certificates based on the size of the safety radius 551 | num_constraints = temp + num_robots*num_obstacles + 4*num_robots 552 | A[0:num_constraints, 0:2*num_robots] = 0 553 | Os[0, 0:num_robots] = np.cos(x[2, :]) 554 | Os[1, 0:num_robots] = np.sin(x[2, :]) 555 | ps[:, 0:num_robots] = x[:2, :] + projection_distance*Os[:, 0:num_robots] 556 | Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] 557 | Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] 558 | Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] 559 | Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] 560 | ret = np.zeros([1,temp]) 561 | 562 | count = 0 563 | 564 | for i in range(num_robots-1): 565 | for j in range(i+1, num_robots): 566 | diff = ps[:, [i]] - ps[:, [j]] 567 | h = np.sum(np.square(diff),0) - safety_radius**2 568 | h_dot_i = 2*diff.T.dot(Ms[:, ((2*i), (2*i+1))].dot(D)) 569 | h_dot_j = -2*diff.T.dot(Ms[:, ((2*j), (2*j+1))].dot(D)) 570 | h_dot_i = np.reshape(h_dot_i, (1,2)) 571 | h_dot_j = np.reshape(h_dot_j, (1,2)) 572 | A[count, ((2*i), (2*i+1))]=h_dot_i 573 | A[count, ((2*j), (2*j+1))]=h_dot_j 574 | b[count] = -barrier_gain*(np.power(h,3)) - np.min(h_dot_i.dot(disturb), 1) - np.min(h_dot_j.dot(disturb), 1) 575 | count += 1 576 | 577 | if obstacles.size != 0: 578 | # Do obstacles 579 | for i in range(num_robots): 580 | diffs = (ps[:, i] - obstacles) 581 | h = np.sum(np.square(diff),0) - safety_radius**2 582 | h_dot_i = 2*diffs*Ms[:, (2*i, 2*i+1)].dot(D) 583 | A[count:count+num_obstacles, ((2*i),(2*i+1))] = h_dot_i 584 | b[count:count+num_obstacles] = -barrier_gain*(np.power(h,3)) - np.min(h_dot_i.dot(disturb), 1) 585 | count = count + num_obstacles 586 | 587 | for k in range(num_robots): 588 | #Pos Y 589 | A[count, (2*k, 2*k+1)] = -Ms[1,(2*k,2*k+1)].dot(D) 590 | b[count] = -0.4*barrier_gain*(boundary_points[3] - safety_radius/2 - ps[1,k])**3; 591 | count += 1 592 | 593 | #Neg Y 594 | A[count, (2*k, 2*k+1)] = Ms[1,(2*k,2*k+1)].dot(D) 595 | b[count] = -0.4*barrier_gain*(-boundary_points[2] - safety_radius/2 + ps[1,k])**3; 596 | count += 1 597 | 598 | #Pos X 599 | A[count, (2*k, 2*k+1)] = -Ms[0,(2*k,2*k+1)].dot(D) 600 | b[count] = -0.4*barrier_gain*(boundary_points[1] - safety_radius/2 - ps[0,k])**3; 601 | count += 1 602 | 603 | #Neg X 604 | A[count, (2*k, 2*k+1)] = Ms[0,(2*k,2*k+1)].dot(D) 605 | b[count] = -0.4*barrier_gain*(-boundary_points[0] - safety_radius/2 + ps[0,k])**3; 606 | count += 1 607 | 608 | # Adding Upper Bounds On Wheels 609 | A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) 610 | b[count:count+2*num_robots] = -wheel_vel_limit 611 | count += 2*num_robots 612 | # # Adding Lower Bounds on Wheels 613 | A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) 614 | b[count:count+2*num_robots] = -wheel_vel_limit 615 | count += 2*num_robots 616 | 617 | # Solve QP program generated earlier 618 | L_all = np.kron(np.eye(num_robots), L) 619 | dxu = np.linalg.inv(D).dot(dxu) # Convert user input to differential drive 620 | vhat = np.reshape(dxu ,(2*num_robots,1), order='F') 621 | H = 2*L_all.T.dot(L_all) 622 | f = -2*vhat.T.dot(L_all.T.dot(L_all)) 623 | 624 | # Alternative Solver 625 | #start = time.time() 626 | vnew = qp(matrix(H), matrix(f.T), -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 627 | #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) 628 | 629 | # vnew = solver2.solve_qp(H, np.float64(f), -A[0:count,0:2*num_robots], -np.array(b[0:count]))[0] 630 | # Initial Guess for Solver at the Next Iteration 631 | # 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); 632 | # Set robot velocities to new velocities 633 | dxu = np.reshape(vnew, (2, -1), order='F') 634 | dxu = D.dot(dxu) 635 | 636 | return dxu 637 | 638 | return robust_barriers 639 | -------------------------------------------------------------------------------- /rps/utilities/barrier_certificates2.py: -------------------------------------------------------------------------------- 1 | from cvxopt import matrix, solvers 2 | import quadprog as solver2 3 | import numpy as np 4 | import time 5 | solvers.options['show_progress'] = False 6 | solvers.options['reltol'] = 1e-2 # was e-2 7 | solvers.options['feastol'] = 1e-2 # was e-4 8 | solvers.options['maxiters'] = 50 # default is 100 9 | 10 | def create_robust_barriers(max_num_obstacles = 100, max_num_robots = 30, d = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, 11 | projection_distance =0.05, gamma = 150, safety_radius = 0.12): # gamma was 150 12 | D = np.matrix([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) 13 | L = np.matrix([[1,0],[0,projection_distance]])* D 14 | disturb = np.matrix([[-d, -d, d, d],[-d, d, d, -d]]) 15 | num_disturbs = np.size(disturb[1,:]) 16 | 17 | #TODO: Take out 4*max_num_robots? 18 | max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacles + 4*max_num_robots 19 | A = np.matrix(np.zeros([max_num_constraints, 2*max_num_robots])) 20 | b = np.matrix(np.zeros([max_num_constraints, 1])) 21 | Os = np.matrix(np.zeros([2,max_num_robots])) 22 | ps = np.matrix(np.zeros([2,max_num_robots])) 23 | Ms = np.matrix(np.zeros([2,2*max_num_robots])) 24 | 25 | def robust_barriers(dxu, x, obstacles): 26 | 27 | num_robots = np.size(dxu[0,:]) 28 | 29 | if obstacles.size != 0: 30 | num_obstacles = np.size(obstacles[0,:]) 31 | else: 32 | num_obstacles = 0 33 | 34 | if(num_robots < 2): 35 | temp = 0 36 | else: 37 | temp = (num_robots**2-num_robots)//2 38 | 39 | 40 | if num_robots == 0: 41 | return [] 42 | 43 | 44 | # Generate constraints for barrier certificates based on the size of the safety radius 45 | num_constraints = temp + num_robots*num_obstacles 46 | A[0:num_constraints, 0:2*num_robots] = 0 47 | Os[0, 0:num_robots] = np.cos(x[2, :]) 48 | Os[1, 0:num_robots] = np.sin(x[2, :]) 49 | ps[:, 0:num_robots] = x[0:2, :] + projection_distance*Os[:, 0:num_robots] 50 | # Ms Real Form 51 | # Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] 52 | # Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] 53 | # Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] 54 | # Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] 55 | # Flipped Ms to be able to perform desired matrix multiplication 56 | Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] 57 | Ms[0, 1:2*num_robots:2] = Os[1, 0:num_robots] 58 | Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] 59 | Ms[1, 0:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] 60 | MDs = (Ms.T * D).T 61 | temp = np.copy(MDs[1, 0:2*num_robots:2]) 62 | MDs[1, 0:2*num_robots:2] = MDs[0, 1:2*num_robots:2] 63 | MDs[0, 1:2*num_robots:2] = temp 64 | 65 | count = 0 66 | 67 | for i in range(num_robots-1): 68 | diffs = ps[:,i] - ps[:, i+1:num_robots] 69 | hs = np.sum(np.square(diffs),0) - safety_radius**2 # 1 by N 70 | h_dot_is = 2*diffs.T*MDs[:,2*i:2*i+2] # N by 2 71 | h_dot_js = np.matrix(np.zeros((2,num_robots - (i+1)))) 72 | h_dot_js[0, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1):2*num_robots:2]), 0) 73 | h_dot_js[1, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1)+1:2*num_robots:2]), 0) 74 | new_constraints = num_robots - i - 1 75 | A[count:count+new_constraints, (2*i):(2*i+2)] = h_dot_is 76 | A[range(count,count+new_constraints), range(2*(i+1),2*num_robots,2)] = h_dot_js[0,:] 77 | A[range(count,count+new_constraints), range(2*(i+1)+1,2*num_robots,2)] = h_dot_js[1,:] 78 | b[count:count+new_constraints] = -gamma*(np.power(hs,3)).T - np.min(h_dot_is*disturb,1) - np.min(h_dot_js.T*disturb,1) 79 | count += new_constraints 80 | 81 | if obstacles.size != 0: 82 | # Do obstacles 83 | for i in range(num_robots): 84 | diffs = (ps[:, i] - obstacles) 85 | h = np.sum(np.square(diffs),0) - safety_radius**2 86 | h_dot_i = 2*diffs.T*MDs[:,2*i:2*i+2] 87 | A[count:count+num_obstacles,(2*i):(2*i+2)] = h_dot_i 88 | b[count:count+num_obstacles] = -gamma*(np.power(h,3)).T - np.min(h_dot_i*disturb, 1) 89 | count = count + num_obstacles 90 | 91 | # Adding Upper Bounds On Wheels 92 | A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) 93 | b[count:count+2*num_robots] = -wheel_vel_limit 94 | count += 2*num_robots 95 | # # Adding Lower Bounds on Wheels 96 | A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) 97 | b[count:count+2*num_robots] = -wheel_vel_limit 98 | count += 2*num_robots 99 | 100 | # Solve QP program generated earlier 101 | L_all = np.kron(np.eye(num_robots), L) 102 | dxu = np.linalg.inv(D)*dxu # Convert user input to differential drive 103 | vhat = np.matrix(np.reshape(dxu ,(2*num_robots,1), order='F')) 104 | H = 2*L_all.T*L_all 105 | f = np.transpose(-2*np.transpose(vhat)*np.transpose(L_all)*L_all) 106 | 107 | # Alternative Solver 108 | #start = time.time() 109 | #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 110 | #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) 111 | 112 | start = time.time() 113 | 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] 114 | print("Time Taken by quadprog: {} s".format(time.time() - start)) 115 | # Initial Guess for Solver at the Next Iteration 116 | # 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); 117 | # Set robot velocities to new velocities 118 | dxu = np.reshape(vnew, (2, num_robots), order='F') 119 | dxu = D*dxu 120 | 121 | return dxu 122 | 123 | return robust_barriers 124 | -------------------------------------------------------------------------------- /rps/utilities/controllers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from rps.utilities.transformations import * 3 | 4 | def create_si_position_controller(x_velocity_gain=1, y_velocity_gain=1, velocity_magnitude_limit=0.15): 5 | """Creates a position controller for single integrators. Drives a single integrator to a point 6 | using a propoertional controller. 7 | 8 | x_velocity_gain - the gain impacting the x (horizontal) velocity of the single integrator 9 | y_velocity_gain - the gain impacting the y (vertical) velocity of the single integrator 10 | velocity_magnitude_limit - the maximum magnitude of the produce velocity vector (should be less than the max linear speed of the platform) 11 | 12 | -> function 13 | """ 14 | 15 | #Check user input types 16 | assert isinstance(x_velocity_gain, (int, float)), "In the function create_si_position_controller, the x linear velocity gain (x_velocity_gain) must be an integer or float. Recieved type %r." % type(x_velocity_gain).__name__ 17 | assert isinstance(y_velocity_gain, (int, float)), "In the function create_si_position_controller, the y linear velocity gain (y_velocity_gain) must be an integer or float. Recieved type %r." % type(y_velocity_gain).__name__ 18 | assert isinstance(velocity_magnitude_limit, (int, float)), "In the function create_si_position_controller, the velocity magnitude limit (y_velocity_gain) must be an integer or float. Recieved type %r." % type(y_velocity_gain).__name__ 19 | 20 | #Check user input ranges/sizes 21 | assert x_velocity_gain > 0, "In the function create_si_position_controller, the x linear velocity gain (x_velocity_gain) must be positive. Recieved %r." % x_velocity_gain 22 | assert y_velocity_gain > 0, "In the function create_si_position_controller, the y linear velocity gain (y_velocity_gain) must be positive. Recieved %r." % y_velocity_gain 23 | assert velocity_magnitude_limit >= 0, "In the function create_si_position_controller, the velocity magnitude limit (velocity_magnitude_limit) must not be negative. Recieved %r." % velocity_magnitude_limit 24 | 25 | gain = np.diag([x_velocity_gain, y_velocity_gain]) 26 | 27 | def si_position_controller(xi, positions): 28 | 29 | """ 30 | xi: 2xN numpy array (of single-integrator states of the robots) 31 | points: 2xN numpy array (of desired points each robot should achieve) 32 | 33 | -> 2xN numpy array (of single-integrator control inputs) 34 | 35 | """ 36 | 37 | #Check user input types 38 | assert isinstance(xi, np.ndarray), "In the si_position_controller function created by the create_si_position_controller function, the single-integrator robot states (xi) must be a numpy array. Recieved type %r." % type(xi).__name__ 39 | assert isinstance(positions, np.ndarray), "In the si_position_controller function created by the create_si_position_controller function, the robot goal points (positions) must be a numpy array. Recieved type %r." % type(positions).__name__ 40 | 41 | #Check user input ranges/sizes 42 | assert xi.shape[0] == 2, "In the si_position_controller function created by the create_si_position_controller function, the dimension of the single-integrator robot states (xi) must be 2 ([x;y]). Recieved dimension %r." % xi.shape[0] 43 | assert positions.shape[0] == 2, "In the si_position_controller function created by the create_si_position_controller function, the dimension of the robot goal points (positions) must be 2 ([x_goal;y_goal]). Recieved dimension %r." % positions.shape[0] 44 | assert xi.shape[1] == positions.shape[1], "In the si_position_controller function created by the create_si_position_controller function, the number of single-integrator robot states (xi) must be equal to the number of robot goal points (positions). Recieved a single integrator current position input array of size %r x %r and desired position array of size %r x %r." % (xi.shape[0], xi.shape[1], positions.shape[0], positions.shape[1]) 45 | 46 | _,N = np.shape(xi) 47 | dxi = np.zeros((2, N)) 48 | 49 | # Calculate control input 50 | dxi[0][:] = x_velocity_gain*(positions[0][:]-xi[0][:]) 51 | dxi[1][:] = y_velocity_gain*(positions[1][:]-xi[1][:]) 52 | 53 | # Threshold magnitude 54 | norms = np.linalg.norm(dxi, axis=0) 55 | idxs = np.where(norms > velocity_magnitude_limit) 56 | if norms[idxs].size != 0: 57 | dxi[:, idxs] *= velocity_magnitude_limit/norms[idxs] 58 | 59 | return dxi 60 | 61 | return si_position_controller 62 | 63 | def create_clf_unicycle_position_controller(linear_velocity_gain=0.8, angular_velocity_gain=3): 64 | """Creates a unicycle model pose controller. Drives the unicycle model to a given position 65 | and orientation. (($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{2 \times N} \to \mathbf{R}^{2 \times N}$) 66 | 67 | linear_velocity_gain - the gain impacting the produced unicycle linear velocity 68 | angular_velocity_gain - the gain impacting the produced unicycle angular velocity 69 | 70 | -> function 71 | """ 72 | 73 | #Check user input types 74 | assert isinstance(linear_velocity_gain, (int, float)), "In the function create_clf_unicycle_position_controller, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ 75 | assert isinstance(angular_velocity_gain, (int, float)), "In the function create_clf_unicycle_position_controller, the angular velocity gain (angular_velocity_gain) must be an integer or float. Recieved type %r." % type(angular_velocity_gain).__name__ 76 | 77 | #Check user input ranges/sizes 78 | assert linear_velocity_gain >= 0, "In the function create_clf_unicycle_position_controller, the linear velocity gain (linear_velocity_gain) must be greater than or equal to zero. Recieved %r." % linear_velocity_gain 79 | assert angular_velocity_gain >= 0, "In the function create_clf_unicycle_position_controller, the angular velocity gain (angular_velocity_gain) must be greater than or equal to zero. Recieved %r." % angular_velocity_gain 80 | 81 | 82 | def position_uni_clf_controller(states, positions): 83 | 84 | """ A position controller for unicycle models. This utilized a control lyapunov function 85 | (CLF) to drive a unicycle system to a desired position. This function operates on unicycle 86 | states and desired positions to return a unicycle velocity command vector. 87 | 88 | states: 3xN numpy array (of unicycle states, [x;y;theta]) 89 | poses: 3xN numpy array (of desired positons, [x_goal;y_goal]) 90 | 91 | -> 2xN numpy array (of unicycle control inputs) 92 | """ 93 | 94 | #Check user input types 95 | assert isinstance(states, np.ndarray), "In the function created by the create_clf_unicycle_position_controller function, the single-integrator robot states (xi) must be a numpy array. Recieved type %r." % type(states).__name__ 96 | assert isinstance(positions, np.ndarray), "In the function created by the create_clf_unicycle_position_controller function, the robot goal points (positions) must be a numpy array. Recieved type %r." % type(positions).__name__ 97 | 98 | #Check user input ranges/sizes 99 | assert states.shape[0] == 3, "In the function created by the create_clf_unicycle_position_controller function, the dimension of the unicycle robot states (states) must be 3 ([x;y;theta]). Recieved dimension %r." % states.shape[0] 100 | assert positions.shape[0] == 2, "In the function created by the create_clf_unicycle_position_controller function, the dimension of the robot goal positions (positions) must be 2 ([x_goal;y_goal]). Recieved dimension %r." % positions.shape[0] 101 | assert states.shape[1] == positions.shape[1], "In the function created by the create_clf_unicycle_position_controller function, the number of unicycle robot states (states) must be equal to the number of robot goal positions (positions). Recieved a current robot pose input array (states) of size %r states %r and desired position array (positions) of size %r states %r." % (states.shape[0], states.shape[1], positions.shape[0], positions.shape[1]) 102 | 103 | 104 | _,N = np.shape(states) 105 | dxu = np.zeros((2, N)) 106 | 107 | pos_error = positions - states[:2][:] 108 | rot_error = np.arctan2(pos_error[1][:],pos_error[0][:]) 109 | dist = np.linalg.norm(pos_error, axis=0) 110 | 111 | dxu[0][:]=linear_velocity_gain*dist*np.cos(rot_error-states[2][:]) 112 | dxu[1][:]=angular_velocity_gain*dist*np.sin(rot_error-states[2][:]) 113 | 114 | return dxu 115 | 116 | return position_uni_clf_controller 117 | 118 | def create_clf_unicycle_pose_controller(approach_angle_gain=1, desired_angle_gain=2.7, rotation_error_gain=1): 119 | """Returns a controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N}$) 120 | that will drive a unicycle-modeled agent to a pose (i.e., position & orientation). This control is based on a control 121 | Lyapunov function. 122 | 123 | approach_angle_gain - affects how the unicycle approaches the desired position 124 | desired_angle_gain - affects how the unicycle approaches the desired angle 125 | rotation_error_gain - affects how quickly the unicycle corrects rotation errors. 126 | 127 | 128 | -> function 129 | """ 130 | 131 | gamma = approach_angle_gain 132 | k = desired_angle_gain 133 | h = rotation_error_gain 134 | 135 | def R(theta): 136 | return np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta),np.cos(theta)]]) 137 | 138 | def pose_uni_clf_controller(states, poses): 139 | 140 | N_states = states.shape[1] 141 | dxu = np.zeros((2,N_states)) 142 | 143 | for i in range(N_states): 144 | translate = R(-poses[2,i]).dot((poses[:2,i]-states[:2,i])) 145 | e = np.linalg.norm(translate) 146 | theta = np.arctan2(translate[1],translate[0]) 147 | alpha = theta - (states[2,i]-poses[2,i]) 148 | alpha = np.arctan2(np.sin(alpha),np.cos(alpha)) 149 | 150 | ca = np.cos(alpha) 151 | sa = np.sin(alpha) 152 | 153 | print(gamma) 154 | print(e) 155 | print(ca) 156 | 157 | dxu[0,i] = gamma* e* ca 158 | dxu[1,i] = k*alpha + gamma*((ca*sa)/alpha)*(alpha + h*theta) 159 | 160 | return dxu 161 | 162 | return pose_uni_clf_controller 163 | 164 | def create_hybrid_unicycle_pose_controller(linear_velocity_gain=1, angular_velocity_gain=2, velocity_magnitude_limit=0.15, angular_velocity_limit=np.pi, position_error=0.05, position_epsilon=0.02, rotation_error=0.05): 165 | '''Returns a controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N}$) 166 | that will drive a unicycle-modeled agent to a pose (i.e., position & orientation). This controller is 167 | based on a hybrid controller that will drive the robot in a straight line to the desired position then rotate 168 | to the desired position. 169 | 170 | linear_velocity_gain - affects how much the linear velocity is scaled based on the position error 171 | angular_velocity_gain - affects how much the angular velocity is scaled based on the heading error 172 | velocity_magnitude_limit - threshold for the max linear velocity that will be achieved by the robot 173 | angular_velocity_limit - threshold for the max rotational velocity that will be achieved by the robot 174 | position_error - the error tolerance for the final position of the robot 175 | position_epsilon - the amount of translational distance that is allowed by the rotation before correcting position again. 176 | rotation_error - the error tolerance for the final orientation of the robot 177 | 178 | ''' 179 | 180 | si_to_uni_dyn = create_si_to_uni_dynamics(linear_velocity_gain=linear_velocity_gain, angular_velocity_limit=angular_velocity_limit) 181 | 182 | def pose_uni_hybrid_controller(states, poses, input_approach_state = np.empty([0,0])): 183 | N=states.shape[1] 184 | dxu = np.zeros((2,N)) 185 | 186 | #This is essentially a hack since default arguments are evaluated only once we will mutate it with each call 187 | if input_approach_state.shape[1] != N: 188 | approach_state = np.ones((1,N))[0] 189 | 190 | for i in range(N): 191 | wrapped = poses[2,i] - states[2,i] 192 | wrapped = np.arctan2(np.sin(wrapped),np.cos(wrapped)) 193 | 194 | dxi = poses[:2,[i]] - states[:2,[i]] 195 | 196 | #Normalize Vector 197 | norm_ = np.linalg.norm(dxi) 198 | 199 | if(norm_ > (position_error - position_epsilon) and approach_state[i]): 200 | if(norm_ > velocity_magnitude_limit): 201 | dxi = velocity_magnitude_limit*dxi/norm_ 202 | dxu[:,[i]] = si_to_uni_dyn(dxi, states[:,[i]]) 203 | elif(np.absolute(wrapped) > rotation_error): 204 | approach_state[i] = 0 205 | if(norm_ > position_error): 206 | approach_state = 1 207 | dxu[0,i] = 0 208 | dxu[1,i] = angular_velocity_gain*wrapped 209 | else: 210 | dxu[:,[i]] = np.zeros((2,1)) 211 | 212 | return dxu 213 | 214 | return pose_uni_hybrid_controller 215 | -------------------------------------------------------------------------------- /rps/utilities/graph.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def cycle_GL(N): 4 | """ Generates a graph Laplacian for a cycle graph 5 | 6 | N: int (number of agents) 7 | 8 | -> NxN numpy array (representing the graph Laplacian) 9 | """ 10 | #Check user input types 11 | assert isinstance(N, int), "In the cycle_GL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ 12 | #Check user input ranges/sizes 13 | assert N > 0, "In the cycle_GL function, number of nodes (N) must be positive. Recieved %r." % N 14 | 15 | ones = np.ones(N-1) 16 | L = 2*np.identity(N) - np.diag(ones, 1) - np.diag(ones, -1) 17 | L[N-1, 0] = -1 18 | L[0, N-1] = -1 19 | 20 | return L 21 | 22 | def lineGL(N): 23 | """ Generates a graph Laplacian for a line graph 24 | 25 | N: int (number of agents) 26 | 27 | -> NxN numpy array (representing the graph Laplacian) 28 | """ 29 | #Check user input types 30 | assert isinstance(N, int), "In the lineGL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ 31 | #Check user input ranges/sizes 32 | assert N > 0, "In the lineGL function, number of nodes (N) must be positive. Recieved %r." % N 33 | 34 | ones = np.ones(N-1) 35 | L = 2*np.identity(N) - np.diag(ones, 1) - np.diag(ones, -1) 36 | L[0,0] = 1 37 | L[N-1,N-1] = 1 38 | 39 | return L 40 | 41 | def completeGL(N): 42 | """ Generates a graph Laplacian for a complete graph 43 | 44 | N: int (number of agents) 45 | 46 | -> NxN numpy array (representing the graph Laplacian) 47 | """ 48 | 49 | #Check user input types 50 | assert isinstance(N, int), "In the completeGL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ 51 | #Check user input ranges/sizes 52 | assert N > 0, "In the completeGL function, number of nodes (N) must be positive. Recieved %r." % N 53 | 54 | L = N*np.identity(N)-np.ones((N,N)) 55 | 56 | return L 57 | 58 | def random_connectedGL(v, e): 59 | """ Generates a Laplacian for a random, connected graph with v verticies 60 | and (v-1) + e edges. 61 | 62 | v: int (number of nodes) 63 | e: number of additional edges 64 | 65 | -> vxv numpy array (representing the graph Laplacian) 66 | """ 67 | 68 | #Check user input types 69 | assert isinstance(v, int), "In the random_connectedGL function, the number of verticies (v) must be an integer. Recieved type %r." % type(v).__name__ 70 | assert isinstance(e, int), "In the random_connectedGL function, the number of additional edges (e) must be an integer. Recieved type %r." % type(e).__name__ 71 | #Check user input ranges/sizes 72 | assert v > 0, "In the random_connectedGL function, number of verticies (v) must be positive. Recieved %r." % v 73 | assert e >= 0, "In the random_connectedGL function, number of additional edges (e) must be greater than or equal to zero. Recieved %r." % e 74 | 75 | 76 | L = np.zeros((v,v)) 77 | 78 | for i in range(1, v): 79 | edge = np.random.randint(i,size=(1,1)) 80 | #Update adjancency relations 81 | L[i,edge] = -1 82 | L[edge,i] = -1 83 | 84 | #Update node degrees 85 | L[i,i] += 1 86 | L[edge,edge] = L[edge,edge]+1 87 | # Because all nodes have at least 1 degree, chosse from only upper diagonal portion 88 | iut = np.triu_indices(v) 89 | iul = np.tril_indices(v) 90 | Ltemp = np.copy(L) 91 | Ltemp[iul] = 1 92 | potEdges = np.where(np.logical_xor(Ltemp, 1)==True) 93 | numEdges = min(e, len(potEdges[0])) 94 | 95 | if numEdges <= 0: 96 | return L 97 | 98 | #Indicies of randomly chosen extra edges 99 | edgeIndicies = np.random.permutation(len(potEdges[0]))[:numEdges] 100 | sz =L.shape 101 | 102 | for index in edgeIndicies: 103 | 104 | #Update adjacency relation 105 | L[potEdges[0][index],potEdges[1][index]] = -1 106 | L[potEdges[1][index],potEdges[0][index]] = -1 107 | 108 | #Update Degree Relation 109 | L[potEdges[0][index],potEdges[0][index]] += 1 110 | L[potEdges[1][index],potEdges[1][index]] += 1 111 | 112 | return L 113 | 114 | def randomGL(v, e): 115 | """ Generates a Laplacian for a random graph with v verticies 116 | and e edges. 117 | 118 | v: int (number of nodes) 119 | e: number of additional edges 120 | 121 | -> vxv numpy array (representing the graph Laplacian) 122 | """ 123 | 124 | L = np.tril(np.ones((v,v))) 125 | 126 | #This works because you can't select diagonals 127 | potEdges = np.where(L==0) 128 | 129 | L = L-L 130 | 131 | numEdges = min(e, len(potEdges[0])) 132 | #Indicies of randomly chosen extra edges 133 | edgeIndicies = np.random.permutation(len(potEdges[0]))[:numEdges] 134 | 135 | for index in edgeIndicies: 136 | 137 | #Update adjacency relation 138 | L[potEdges[0][index],potEdges[1][index]] = -1 139 | L[potEdges[1][index],potEdges[0][index]] = -1 140 | 141 | #Update Degree Relation 142 | L[potEdges[0][index],potEdges[0][index]] += 1 143 | L[potEdges[1][index],potEdges[1][index]] += 1 144 | 145 | return L 146 | 147 | 148 | def topological_neighbors(L, agent): 149 | """ Returns the neighbors of a particular agent using the graph Laplacian 150 | 151 | L: NxN numpy array (representing the graph Laplacian) 152 | agent: int (agent: 0 - N-1) 153 | 154 | -> 1xM numpy array (with M neighbors) 155 | """ 156 | #Check user input types 157 | assert isinstance(L, np.ndarray), "In the topological_neighbors function, the graph Laplacian (L) must be a numpy ndarray. Recieved type %r." % type(L).__name__ 158 | assert isinstance(agent, int), "In the topological_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ 159 | 160 | #Check user input ranges/sizes 161 | assert agent >= 0, "In the topological_neighbors function, the agent number (agent) must be greater than or equal to zero. Recieved %r." % agent 162 | assert agent <= L.shape[0], "In the topological_neighbors function, the agent number (agent) must be within the dimension of the provided Laplacian (L). Recieved agent number %r and Laplactian size %r by %r." % (agent, L.shape[0], L.shape[1]) 163 | 164 | row = L[agent, :] 165 | row[agent]=0 166 | # Since L = D - A 167 | return np.where(row != 0)[0] 168 | 169 | def delta_disk_neighbors(poses, agent, delta): 170 | ''' Returns the agents within the 2-norm of the supplied agent (not including the agent itself) 171 | poses: 3xN numpy array (representing the unicycle statese of the robots) 172 | agent: int (agent whose neighbors within a radius will be returned) 173 | delta: float (radius of delta disk considered) 174 | 175 | -> 1xM numpy array (with M neighbors) 176 | 177 | ''' 178 | #Check user input types 179 | assert isinstance(poses, np.ndarray), "In the delta_disk_neighbors function, the robot poses (poses) must be a numpy ndarray. Recieved type %r." % type(poses).__name__ 180 | assert isinstance(agent, int), "In the delta_disk_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ 181 | assert isinstance(delta, (int,float)), "In the delta_disk_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ 182 | 183 | #Check user input ranges/sizes 184 | assert agent >= 0, "In the delta_disk_neighbors function, the agent number (agent) must be greater than or equal to zero. Recieved %r." % agent 185 | assert delta >=0, "In the delta_disk_neighbors function, the sensing/communication radius (delta) must be greater than or equal to zero. Recieved %r." % delta 186 | assert agent <= poses.shape[1], "In the delta_disk_neighbors function, the agent number (agent) must be within the dimension of the provided poses. Recieved agent number %r and poses for %r agents." % (agent, poses.shape[1]) 187 | 188 | 189 | 190 | N = poses.shape[1] 191 | agents = np.arange(N) 192 | 193 | within_distance = [np.linalg.norm(poses[:2,x]-poses[:2,agent])<=delta for x in agents] 194 | within_distance[agent] = False 195 | return agents[within_distance] -------------------------------------------------------------------------------- /rps/utilities/misc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | def generate_initial_conditions(N, spacing=0.3, width=3, height=1.8): 6 | """Generates random initial conditions in an area of the specified 7 | width and height at the required spacing. 8 | 9 | N: int (number of agents) 10 | spacing: double (how far apart positions can be) 11 | width: double (width of area) 12 | height: double (height of area) 13 | 14 | -> 3xN numpy array (of poses) 15 | """ 16 | #Check user input types 17 | assert isinstance(N, int), "In the function generate_initial_conditions, the number of robots (N) to generate intial conditions for must be an integer. Recieved type %r." % type(N).__name__ 18 | assert isinstance(spacing, (float,int)), "In the function generate_initial_conditions, the minimum spacing between robots (spacing) must be an integer or float. Recieved type %r." % type(spacing).__name__ 19 | assert isinstance(width, (float,int)), "In the function generate_initial_conditions, the width of the area to place robots randomly (width) must be an integer or float. Recieved type %r." % type(width).__name__ 20 | assert isinstance(height, (float,int)), "In the function generate_initial_conditions, the height of the area to place robots randomly (width) must be an integer or float. Recieved type %r." % type(height).__name__ 21 | 22 | #Check user input ranges/sizes 23 | assert N > 0, "In the function generate_initial_conditions, the number of robots to generate initial conditions for (N) must be positive. Recieved %r." % N 24 | assert spacing > 0, "In the function generate_initial_conditions, the spacing between robots (spacing) must be positive. Recieved %r." % spacing 25 | assert width > 0, "In the function generate_initial_conditions, the width of the area to initialize robots randomly (width) must be positive. Recieved %r." % width 26 | assert height >0, "In the function generate_initial_conditions, the height of the area to initialize robots randomly (height) must be positive. Recieved %r." % height 27 | 28 | x_range = int(np.floor(width/spacing)) 29 | y_range = int(np.floor(height/spacing)) 30 | 31 | assert x_range != 0, "In the function generate_initial_conditions, the space between robots (space) is too large compared to the width of the area robots are randomly initialized in (width)." 32 | assert y_range != 0, "In the function generate_initial_conditions, the space between robots (space) is too large compared to the height of the area robots are randomly initialized in (height)." 33 | assert x_range*y_range > N, "In the function generate_initial_conditions, it is impossible to place %r robots within a %r x %r meter area with a spacing of %r meters." % (N, width, height, spacing) 34 | 35 | choices = (np.random.choice(x_range*y_range, N, replace=False)+1) 36 | 37 | poses = np.zeros((3, N)) 38 | 39 | for i, c in enumerate(choices): 40 | x,y = divmod(c, y_range) 41 | poses[0, i] = x*spacing - width/2 42 | poses[1, i] = y*spacing - height/2 43 | poses[2, i] = np.random.rand()*2*np.pi - np.pi 44 | 45 | return poses 46 | 47 | def at_pose(states, poses, position_error=0.05, rotation_error=0.2): 48 | """Checks whether robots are "close enough" to poses 49 | 50 | states: 3xN numpy array (of unicycle states) 51 | poses: 3xN numpy array (of desired states) 52 | 53 | -> 1xN numpy index array (of agents that are close enough) 54 | """ 55 | #Check user input types 56 | assert isinstance(states, np.ndarray), "In the at_pose function, the robot current state argument (states) must be a numpy ndarray. Recieved type %r." % type(states).__name__ 57 | assert isinstance(poses, np.ndarray), "In the at_pose function, the checked pose argument (poses) must be a numpy ndarray. Recieved type %r." % type(poses).__name__ 58 | assert isinstance(position_error, (float,int)), "In the at_pose function, the allowable position error argument (position_error) must be an integer or float. Recieved type %r." % type(position_error).__name__ 59 | assert isinstance(rotation_error, (float,int)), "In the at_pose function, the allowable angular error argument (rotation_error) must be an integer or float. Recieved type %r." % type(rotation_error).__name__ 60 | 61 | #Check user input ranges/sizes 62 | assert states.shape[0] == 3, "In the at_pose function, the dimension of the state of each robot must be 3 ([x;y;theta]). Recieved %r." % states.shape[0] 63 | assert poses.shape[0] == 3, "In the at_pose function, the dimension of the checked pose of each robot must be 3 ([x;y;theta]). Recieved %r." % poses.shape[0] 64 | assert states.shape == poses.shape, "In the at_pose function, the robot current state and checked pose inputs must be the same size (3xN, where N is the number of robots being checked). Recieved a state array of size %r x %r and checked pose array of size %r x %r." % (states.shape[0], states.shape[1], poses.shape[0], poses.shape[1]) 65 | 66 | # Calculate rotation errors with angle wrapping 67 | res = states[2, :] - poses[2, :] 68 | res = np.abs(np.arctan2(np.sin(res), np.cos(res))) 69 | 70 | # Calculate position errors 71 | pes = np.linalg.norm(states[:2, :] - poses[:2, :], 2, 0) 72 | 73 | # Determine which agents are done 74 | done = np.nonzero((res <= rotation_error) & (pes <= position_error)) 75 | 76 | return done 77 | 78 | def at_position(states, points, position_error=0.02): 79 | """Checks whether robots are "close enough" to desired position 80 | 81 | states: 3xN numpy array (of unicycle states) 82 | points: 2xN numpy array (of desired points) 83 | 84 | -> 1xN numpy index array (of agents that are close enough) 85 | """ 86 | 87 | #Check user input types 88 | assert isinstance(states, np.ndarray), "In the at_position function, the robot current state argument (states) must be a numpy ndarray. Recieved type %r." % type(states).__name__ 89 | assert isinstance(points, np.ndarray), "In the at_position function, the desired pose argument (poses) must be a numpy ndarray. Recieved type %r." % type(points).__name__ 90 | assert isinstance(position_error, (float,int)), "In the at_position function, the allowable position error argument (position_error) must be an integer or float. Recieved type %r." % type(position_error).__name__ 91 | 92 | #Check user input ranges/sizes 93 | assert states.shape[0] == 3, "In the at_position function, the dimension of the state of each robot (states) must be 3. Recieved %r." % states.shape[0] 94 | assert points.shape[0] == 2, "In the at_position function, the dimension of the checked position for each robot (points) must be 2. Recieved %r." % points.shape[0] 95 | assert states.shape[1] == poses.shape[1], "In the at_position function, the number of checked points (points) must match the number of robot states provided (states). Recieved a state array of size %r x %r and desired pose array of size %r x %r." % (states.shape[0], states.shape[1], points.shape[0], points.shape[1]) 96 | 97 | # Calculate position errors 98 | pes = np.linalg.norm(states[:2, :] - points, 2, 0) 99 | 100 | # Determine which agents are done 101 | done = np.nonzero((pes <= position_error)) 102 | 103 | return done 104 | 105 | def determine_marker_size(robotarium_instance, marker_size_meters): 106 | 107 | # Get the x and y dimension of the robotarium figure window in pixels 108 | fig_dim_pixels = robotarium_instance.axes.transData.transform(np.array([[robotarium_instance.boundaries[2]],[robotarium_instance.boundaries[3]]])) 109 | 110 | # Determine the ratio of the robot size to the x-axis (the axis are 111 | # normalized so you could do this with y and figure height as well). 112 | marker_ratio = (marker_size_meters)/(robotarium_instance.boundaries[2]) 113 | 114 | # Determine the marker size in points so it fits the window. Note: This is squared 115 | # as marker sizes are areas. 116 | return (fig_dim_pixels[0,0] * marker_ratio)**2. 117 | 118 | 119 | def determine_font_size(robotarium_instance, font_height_meters): 120 | 121 | # Get the x and y dimension of the robotarium figure window in pixels 122 | y1, y2 = robotarium_instance.axes.get_window_extent().get_points()[:,1] 123 | 124 | # Determine the ratio of the robot size to the y-axis. 125 | font_ratio = (y2-y1)/(robotarium_instance.boundaries[2]) 126 | 127 | # Determine the font size in points so it fits the window. 128 | return (font_ratio*font_height_meters) 129 | -------------------------------------------------------------------------------- /rps/utilities/transformations.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def create_si_to_uni_dynamics(linear_velocity_gain=1, angular_velocity_limit=np.pi): 4 | """ Returns a function mapping from single-integrator to unicycle dynamics with angular velocity magnitude restrictions. 5 | 6 | linear_velocity_gain: Gain for unicycle linear velocity 7 | angular_velocity_limit: Limit for angular velocity (i.e., |w| < angular_velocity_limit) 8 | 9 | -> function 10 | """ 11 | 12 | #Check user input types 13 | assert isinstance(linear_velocity_gain, (int, float)), "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ 14 | assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ 15 | 16 | #Check user input ranges/sizes 17 | assert linear_velocity_gain > 0, "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be positive. Recieved %r." % linear_velocity_gain 18 | assert angular_velocity_limit >= 0, "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must not be negative. Recieved %r." % angular_velocity_limit 19 | 20 | 21 | def si_to_uni_dyn(dxi, poses): 22 | """A mapping from single-integrator to unicycle dynamics. 23 | 24 | dxi: 2xN numpy array with single-integrator control inputs 25 | poses: 2xN numpy array with single-integrator poses 26 | 27 | -> 2xN numpy array of unicycle control inputs 28 | """ 29 | 30 | #Check user input types 31 | assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 32 | assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ 33 | 34 | #Check user input ranges/sizes 35 | assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 36 | assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] 37 | assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) 38 | 39 | M,N = np.shape(dxi) 40 | 41 | a = np.cos(poses[2, :]) 42 | b = np.sin(poses[2, :]) 43 | 44 | dxu = np.zeros((2, N)) 45 | dxu[0, :] = linear_velocity_gain*(a*dxi[0, :] + b*dxi[1, :]) 46 | dxu[1, :] = angular_velocity_limit*np.arctan2(-b*dxi[0, :] + a*dxi[1, :], dxu[0, :])/(np.pi/2) 47 | 48 | return dxu 49 | 50 | return si_to_uni_dyn 51 | 52 | def create_si_to_uni_dynamics_with_backwards_motion(linear_velocity_gain=1, angular_velocity_limit=np.pi): 53 | """ Returns a function mapping from single-integrator dynamics to unicycle dynamics. This implementation of 54 | the mapping allows for robots to drive backwards if that direction of linear velocity requires less rotation. 55 | 56 | linear_velocity_gain: Gain for unicycle linear velocity 57 | angular_velocity_limit: Limit for angular velocity (i.e., |w| < angular_velocity_limit) 58 | 59 | """ 60 | 61 | #Check user input types 62 | assert isinstance(linear_velocity_gain, (int, float)), "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ 63 | assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ 64 | 65 | #Check user input ranges/sizes 66 | assert linear_velocity_gain > 0, "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be positive. Recieved %r." % linear_velocity_gain 67 | assert angular_velocity_limit >= 0, "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must not be negative. Recieved %r." % angular_velocity_limit 68 | 69 | 70 | def si_to_uni_dyn(dxi, poses): 71 | """A mapping from single-integrator to unicycle dynamics. 72 | 73 | dxi: 2xN numpy array with single-integrator control inputs 74 | poses: 2xN numpy array with single-integrator poses 75 | 76 | -> 2xN numpy array of unicycle control inputs 77 | """ 78 | 79 | #Check user input types 80 | assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 81 | assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ 82 | 83 | #Check user input ranges/sizes 84 | assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 85 | assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] 86 | assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) 87 | 88 | M,N = np.shape(dxi) 89 | 90 | a = np.cos(poses[2, :]) 91 | b = np.sin(poses[2, :]) 92 | 93 | dxu = np.zeros((2, N)) 94 | dxu[0, :] = linear_velocity_gain*(a*dxi[0, :] + b*dxi[1, :]) 95 | dxu[1, :] = angular_velocity_limit*np.arctan2(-b*dxi[0, :] + a*dxi[1, :], dxu[0, :])/(np.pi/2) 96 | 97 | return dxu 98 | 99 | return si_to_uni_dyn 100 | 101 | def create_si_to_uni_mapping(projection_distance=0.05, angular_velocity_limit = np.pi): 102 | """Creates two functions for mapping from single integrator dynamics to 103 | unicycle dynamics and unicycle states to single integrator states. 104 | 105 | This mapping is done by placing a virtual control "point" in front of 106 | the unicycle. 107 | 108 | projection_distance: How far ahead to place the point 109 | angular_velocity_limit: The maximum angular velocity that can be provided 110 | 111 | -> (function, function) 112 | """ 113 | 114 | #Check user input types 115 | assert isinstance(projection_distance, (int, float)), "In the function create_si_to_uni_mapping, the projection distance of the new control point (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ 116 | assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_mapping, the maximum angular velocity command (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ 117 | 118 | #Check user input ranges/sizes 119 | assert projection_distance > 0, "In the function create_si_to_uni_mapping, the projection distance of the new control point (projection_distance) must be positive. Recieved %r." % projection_distance 120 | assert projection_distance >= 0, "In the function create_si_to_uni_mapping, the maximum angular velocity command (angular_velocity_limit) must be greater than or equal to zero. Recieved %r." % angular_velocity_limit 121 | 122 | def si_to_uni_dyn(dxi, poses): 123 | """Takes single-integrator velocities and transforms them to unicycle 124 | control inputs. 125 | 126 | dxi: 2xN numpy array of single-integrator control inputs 127 | poses: 3xN numpy array of unicycle poses 128 | 129 | -> 2xN numpy array of unicycle control inputs 130 | """ 131 | 132 | #Check user input types 133 | assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ 134 | assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ 135 | 136 | #Check user input ranges/sizes 137 | assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] 138 | assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] 139 | assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) 140 | 141 | 142 | M,N = np.shape(dxi) 143 | 144 | cs = np.cos(poses[2, :]) 145 | ss = np.sin(poses[2, :]) 146 | 147 | dxu = np.zeros((2, N)) 148 | dxu[0, :] = (cs*dxi[0, :] + ss*dxi[1, :]) 149 | dxu[1, :] = (1/projection_distance)*(-ss*dxi[0, :] + cs*dxi[1, :]) 150 | 151 | #Impose angular velocity cap. 152 | dxu[1,dxu[1,:]>angular_velocity_limit] = angular_velocity_limit 153 | dxu[1,dxu[1,:]<-angular_velocity_limit] = -angular_velocity_limit 154 | 155 | return dxu 156 | 157 | def uni_to_si_states(poses): 158 | """Takes unicycle states and returns single-integrator states 159 | 160 | poses: 3xN numpy array of unicycle states 161 | 162 | -> 2xN numpy array of single-integrator states 163 | """ 164 | 165 | _,N = np.shape(poses) 166 | 167 | si_states = np.zeros((2, N)) 168 | si_states[0, :] = poses[0, :] + projection_distance*np.cos(poses[2, :]) 169 | si_states[1, :] = poses[1, :] + projection_distance*np.sin(poses[2, :]) 170 | 171 | return si_states 172 | 173 | return si_to_uni_dyn, uni_to_si_states 174 | 175 | def create_uni_to_si_dynamics(projection_distance=0.05): 176 | """Creates two functions for mapping from unicycle dynamics to single 177 | integrator dynamics and single integrator states to unicycle states. 178 | 179 | This mapping is done by placing a virtual control "point" in front of 180 | the unicycle. 181 | 182 | projection_distance: How far ahead to place the point 183 | 184 | -> function 185 | """ 186 | 187 | #Check user input types 188 | assert isinstance(projection_distance, (int, float)), "In the function create_uni_to_si_dynamics, the projection distance of the new control point (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ 189 | 190 | #Check user input ranges/sizes 191 | assert projection_distance > 0, "In the function create_uni_to_si_dynamics, the projection distance of the new control point (projection_distance) must be positive. Recieved %r." % projection_distance 192 | 193 | 194 | def uni_to_si_dyn(dxu, poses): 195 | """A function for converting from unicycle to single-integrator dynamics. 196 | Utilizes a virtual point placed in front of the unicycle. 197 | 198 | dxu: 2xN numpy array of unicycle control inputs 199 | poses: 3xN numpy array of unicycle poses 200 | projection_distance: How far ahead of the unicycle model to place the point 201 | 202 | -> 2xN numpy array of single-integrator control inputs 203 | """ 204 | 205 | #Check user input types 206 | assert isinstance(dxu, np.ndarray), "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the unicycle velocity inputs (dxu) must be a numpy array. Recieved type %r." % type(dxi).__name__ 207 | assert isinstance(poses, np.ndarray), "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ 208 | 209 | #Check user input ranges/sizes 210 | assert dxu.shape[0] == 2, "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the dimension of the unicycle velocity inputs (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] 211 | assert poses.shape[0] == 3, "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] 212 | assert dxu.shape[1] == poses.shape[1], "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the number of unicycle velocity inputs must be equal to the number of current robot poses. Recieved a unicycle velocity input array of size %r x %r and current pose array of size %r x %r." % (dxu.shape[0], dxu.shape[1], poses.shape[0], poses.shape[1]) 213 | 214 | 215 | M,N = np.shape(dxu) 216 | 217 | cs = np.cos(poses[2, :]) 218 | ss = np.sin(poses[2, :]) 219 | 220 | dxi = np.zeros((2, N)) 221 | dxi[0, :] = (cs*dxu[0, :] - projection_distance*ss*dxu[1, :]) 222 | dxi[1, :] = (ss*dxu[0, :] + projection_distance*cs*dxu[1, :]) 223 | 224 | return dxi 225 | 226 | return uni_to_si_dyn -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | from numpy.random import gamma 2 | import rps.robotarium as robotarium 3 | # from rps.utilities.transformations import * 4 | # from rps.utilities.barrier_certificates import * 5 | # from rps.utilities.misc import * 6 | # from rps.utilities.controllers import * 7 | from clf_cbf_nmpc import CLF_CBF_NMPC,Simple_Catcher 8 | import numpy as np 9 | from observer import Observer 10 | from estimator import Estimator 11 | 12 | 13 | 14 | # N = 2 15 | N = 50 16 | 17 | a = np.array([[-1.2, -1.2 ,0]]).T 18 | # a = np.array([[0, 0 ,0]]).T 19 | d = np.array([[0.1, 0.35, -3.14]]).T # 20 | initial_conditions = a 21 | for idx in range(1, N): 22 | initial_conditions = np.concatenate((initial_conditions, np.array([[2.6*np.random.rand()-1.0, 2.6*np.random.rand()-1.0, 6.28*np.random.rand()-3.14]]).T), axis=1) 23 | # print('Initial conditions:') 24 | # print(initial_conditions) 25 | 26 | r = robotarium.Robotarium(number_of_robots=N, show_figure=True, initial_conditions=initial_conditions, sim_in_real_time=True) 27 | 28 | 29 | def is_done(all_states): 30 | self_state = x[0] 31 | other_states = x[1:] 32 | 33 | # Check boundaries 34 | if(self_state[1]>1.5 or self_state[1]<-1.5 or self_state[0]>1.5 or self_state[0]<-1.5): 35 | print('Out of boundaries !!') 36 | return True 37 | # Reached goal? 38 | if (0.7<=self_state[0]<=1.5 and 0.7<=self_state[1]<=1.5): 39 | print('Reach goal successfully!') 40 | return True 41 | 42 | for idx in range(np.size(other_states, 0)): 43 | # if(other_states[idx][0]>1.5 or other_states[idx][0]<-1.5 or other_states[idx][1]>1.5 or other_states[idx][1]<-1.5 ): 44 | # print('Vehicle %d is out of boundaries !!' % idx+1) 45 | # return True 46 | distSqr = (self_state[0]-other_states[idx][0])**2 + (self_state[1]-other_states[idx][1])**2 47 | if distSqr < (0.2)**2: 48 | print('Get caught, mission failed !') 49 | return True 50 | 51 | return False 52 | 53 | 54 | x = r.get_poses().T 55 | r.step() 56 | 57 | i=0 58 | times = 0 59 | 60 | obsrvr = Observer(x, 0.1, 6) 61 | 62 | mpc_horizon = 10 63 | T = 0.1 64 | m_cbf = 8 65 | m_clf = 0 66 | gamma_k = 0.25 67 | alpha_k = 0.1 68 | clf_cbf_nmpc_solver = CLF_CBF_NMPC(mpc_horizon, T, m_cbf, m_clf, gamma_k, alpha_k) 69 | 70 | while (is_done(x)==False): 71 | # print('\n----------------------------------------------------------') 72 | # print("Iteration %d" % times) 73 | 74 | x = r.get_poses().T 75 | 76 | # Observe & Predict 77 | 78 | obsrvr.feed(x) 79 | f = lambda x_, u_: x_-x_ + u_ 80 | # print(obsrvr.vel[1:]) 81 | estmtr = Estimator(x[1:], obsrvr.vel[1:], f, 0.1, 10) 82 | estmtr.predict() 83 | # print(estmtr.predict_states) 84 | global_states_sol, controls_sol, local_states_sol = clf_cbf_nmpc_solver.solve(x[0], [1.4, 1.4, 0], np.concatenate((np.array([obsrvr.states[1:]]), estmtr.predict_states), axis = 0)) 85 | attacker_u = controls_sol[0] 86 | # attacker_u = np.array([0.2, 0.1]) 87 | 88 | # defender_u = Simple_Catcher(x[0],x[1]) 89 | 90 | dxu = np.zeros([N,2]) 91 | dxu[0] = np.array([attacker_u[0],attacker_u[1]]) 92 | 93 | for idx in range(1, N): 94 | # defender_u = Simple_Catcher(x[0],x[idx]) 95 | # dxu[idx] = defender_u 96 | # dxu[idx] = np.array([0, 0]) 97 | dxu[idx] = np.array([0.15, 0.1]) 98 | # for idx in range(3, N) 99 | # defender_u = Simple_Catcher(x[0],x[idx]) 100 | # dxu[idx] = defender_u 101 | # dxu[idx] = np.array([0.2, 0.02]) 102 | 103 | r.set_velocities(np.arange(N), dxu.T) 104 | 105 | times+=1 106 | i+=1 107 | r.step() 108 | # print('----------------------------------------------------------\n') 109 | 110 | r.call_at_scripts_end() 111 | --------------------------------------------------------------------------------