1from sympy.core.backend import eye, Matrix, zeros 2from sympy.physics.mechanics import dynamicsymbols 3from sympy.physics.mechanics.functions import find_dynamicsymbols 4 5__all__ = ['SymbolicSystem'] 6 7 8class SymbolicSystem: 9 """SymbolicSystem is a class that contains all the information about a 10 system in a symbolic format such as the equations of motions and the bodies 11 and loads in the system. 12 13 There are three ways that the equations of motion can be described for 14 Symbolic System: 15 16 17 [1] Explicit form where the kinematics and dynamics are combined 18 x' = F_1(x, t, r, p) 19 20 [2] Implicit form where the kinematics and dynamics are combined 21 M_2(x, p) x' = F_2(x, t, r, p) 22 23 [3] Implicit form where the kinematics and dynamics are separate 24 M_3(q, p) u' = F_3(q, u, t, r, p) 25 q' = G(q, u, t, r, p) 26 27 where 28 29 x : states, e.g. [q, u] 30 t : time 31 r : specified (exogenous) inputs 32 p : constants 33 q : generalized coordinates 34 u : generalized speeds 35 F_1 : right hand side of the combined equations in explicit form 36 F_2 : right hand side of the combined equations in implicit form 37 F_3 : right hand side of the dynamical equations in implicit form 38 M_2 : mass matrix of the combined equations in implicit form 39 M_3 : mass matrix of the dynamical equations in implicit form 40 G : right hand side of the kinematical differential equations 41 42 Parameters 43 ========== 44 45 coord_states : ordered iterable of functions of time 46 This input will either be a collection of the coordinates or states 47 of the system depending on whether or not the speeds are also 48 given. If speeds are specified this input will be assumed to 49 be the coordinates otherwise this input will be assumed to 50 be the states. 51 52 right_hand_side : Matrix 53 This variable is the right hand side of the equations of motion in 54 any of the forms. The specific form will be assumed depending on 55 whether a mass matrix or coordinate derivatives are given. 56 57 speeds : ordered iterable of functions of time, optional 58 This is a collection of the generalized speeds of the system. If 59 given it will be assumed that the first argument (coord_states) 60 will represent the generalized coordinates of the system. 61 62 mass_matrix : Matrix, optional 63 The matrix of the implicit forms of the equations of motion (forms 64 [2] and [3]). The distinction between the forms is determined by 65 whether or not the coordinate derivatives are passed in. If 66 they are given form [3] will be assumed otherwise form [2] is 67 assumed. 68 69 coordinate_derivatives : Matrix, optional 70 The right hand side of the kinematical equations in explicit form. 71 If given it will be assumed that the equations of motion are being 72 entered in form [3]. 73 74 alg_con : Iterable, optional 75 The indexes of the rows in the equations of motion that contain 76 algebraic constraints instead of differential equations. If the 77 equations are input in form [3], it will be assumed the indexes are 78 referencing the mass_matrix/right_hand_side combination and not the 79 coordinate_derivatives. 80 81 output_eqns : Dictionary, optional 82 Any output equations that are desired to be tracked are stored in a 83 dictionary where the key corresponds to the name given for the 84 specific equation and the value is the equation itself in symbolic 85 form 86 87 coord_idxs : Iterable, optional 88 If coord_states corresponds to the states rather than the 89 coordinates this variable will tell SymbolicSystem which indexes of 90 the states correspond to generalized coordinates. 91 92 speed_idxs : Iterable, optional 93 If coord_states corresponds to the states rather than the 94 coordinates this variable will tell SymbolicSystem which indexes of 95 the states correspond to generalized speeds. 96 97 bodies : iterable of Body/Rigidbody objects, optional 98 Iterable containing the bodies of the system 99 100 loads : iterable of load instances (described below), optional 101 Iterable containing the loads of the system where forces are given 102 by (point of application, force vector) and torques are given by 103 (reference frame acting upon, torque vector). Ex [(point, force), 104 (ref_frame, torque)] 105 106 Attributes 107 ========== 108 109 coordinates : Matrix, shape(n, 1) 110 This is a matrix containing the generalized coordinates of the system 111 112 speeds : Matrix, shape(m, 1) 113 This is a matrix containing the generalized speeds of the system 114 115 states : Matrix, shape(o, 1) 116 This is a matrix containing the state variables of the system 117 118 alg_con : List 119 This list contains the indices of the algebraic constraints in the 120 combined equations of motion. The presence of these constraints 121 requires that a DAE solver be used instead of an ODE solver. 122 If the system is given in form [3] the alg_con variable will be 123 adjusted such that it is a representation of the combined kinematics 124 and dynamics thus make sure it always matches the mass matrix 125 entered. 126 127 dyn_implicit_mat : Matrix, shape(m, m) 128 This is the M matrix in form [3] of the equations of motion (the mass 129 matrix or generalized inertia matrix of the dynamical equations of 130 motion in implicit form). 131 132 dyn_implicit_rhs : Matrix, shape(m, 1) 133 This is the F vector in form [3] of the equations of motion (the right 134 hand side of the dynamical equations of motion in implicit form). 135 136 comb_implicit_mat : Matrix, shape(o, o) 137 This is the M matrix in form [2] of the equations of motion. 138 This matrix contains a block diagonal structure where the top 139 left block (the first rows) represent the matrix in the 140 implicit form of the kinematical equations and the bottom right 141 block (the last rows) represent the matrix in the implicit form 142 of the dynamical equations. 143 144 comb_implicit_rhs : Matrix, shape(o, 1) 145 This is the F vector in form [2] of the equations of motion. The top 146 part of the vector represents the right hand side of the implicit form 147 of the kinemaical equations and the bottom of the vector represents the 148 right hand side of the implicit form of the dynamical equations of 149 motion. 150 151 comb_explicit_rhs : Matrix, shape(o, 1) 152 This vector represents the right hand side of the combined equations of 153 motion in explicit form (form [1] from above). 154 155 kin_explicit_rhs : Matrix, shape(m, 1) 156 This is the right hand side of the explicit form of the kinematical 157 equations of motion as can be seen in form [3] (the G matrix). 158 159 output_eqns : Dictionary 160 If output equations were given they are stored in a dictionary where 161 the key corresponds to the name given for the specific equation and 162 the value is the equation itself in symbolic form 163 164 bodies : Tuple 165 If the bodies in the system were given they are stored in a tuple for 166 future access 167 168 loads : Tuple 169 If the loads in the system were given they are stored in a tuple for 170 future access. This includes forces and torques where forces are given 171 by (point of application, force vector) and torques are given by 172 (reference frame acted upon, torque vector). 173 174 Example 175 ======= 176 177 As a simple example, the dynamics of a simple pendulum will be input into a 178 SymbolicSystem object manually. First some imports will be needed and then 179 symbols will be set up for the length of the pendulum (l), mass at the end 180 of the pendulum (m), and a constant for gravity (g). :: 181 182 >>> from sympy import Matrix, sin, symbols 183 >>> from sympy.physics.mechanics import dynamicsymbols, SymbolicSystem 184 >>> l, m, g = symbols('l m g') 185 186 The system will be defined by an angle of theta from the vertical and a 187 generalized speed of omega will be used where omega = theta_dot. :: 188 189 >>> theta, omega = dynamicsymbols('theta omega') 190 191 Now the equations of motion are ready to be formed and passed to the 192 SymbolicSystem object. :: 193 194 >>> kin_explicit_rhs = Matrix([omega]) 195 >>> dyn_implicit_mat = Matrix([l**2 * m]) 196 >>> dyn_implicit_rhs = Matrix([-g * l * m * sin(theta)]) 197 >>> symsystem = SymbolicSystem([theta], dyn_implicit_rhs, [omega], 198 ... dyn_implicit_mat) 199 200 Notes 201 ===== 202 203 m : number of generalized speeds 204 n : number of generalized coordinates 205 o : number of states 206 207 """ 208 209 def __init__(self, coord_states, right_hand_side, speeds=None, 210 mass_matrix=None, coordinate_derivatives=None, alg_con=None, 211 output_eqns={}, coord_idxs=None, speed_idxs=None, bodies=None, 212 loads=None): 213 """Initializes a SymbolicSystem object""" 214 215 # Extract information on speeds, coordinates and states 216 if speeds is None: 217 self._states = Matrix(coord_states) 218 219 if coord_idxs is None: 220 self._coordinates = None 221 else: 222 coords = [coord_states[i] for i in coord_idxs] 223 self._coordinates = Matrix(coords) 224 225 if speed_idxs is None: 226 self._speeds = None 227 else: 228 speeds_inter = [coord_states[i] for i in speed_idxs] 229 self._speeds = Matrix(speeds_inter) 230 else: 231 self._coordinates = Matrix(coord_states) 232 self._speeds = Matrix(speeds) 233 self._states = self._coordinates.col_join(self._speeds) 234 235 # Extract equations of motion form 236 if coordinate_derivatives is not None: 237 self._kin_explicit_rhs = coordinate_derivatives 238 self._dyn_implicit_rhs = right_hand_side 239 self._dyn_implicit_mat = mass_matrix 240 self._comb_implicit_rhs = None 241 self._comb_implicit_mat = None 242 self._comb_explicit_rhs = None 243 elif mass_matrix is not None: 244 self._kin_explicit_rhs = None 245 self._dyn_implicit_rhs = None 246 self._dyn_implicit_mat = None 247 self._comb_implicit_rhs = right_hand_side 248 self._comb_implicit_mat = mass_matrix 249 self._comb_explicit_rhs = None 250 else: 251 self._kin_explicit_rhs = None 252 self._dyn_implicit_rhs = None 253 self._dyn_implicit_mat = None 254 self._comb_implicit_rhs = None 255 self._comb_implicit_mat = None 256 self._comb_explicit_rhs = right_hand_side 257 258 # Set the remainder of the inputs as instance attributes 259 if alg_con is not None and coordinate_derivatives is not None: 260 alg_con = [i + len(coordinate_derivatives) for i in alg_con] 261 self._alg_con = alg_con 262 self.output_eqns = output_eqns 263 264 # Change the body and loads iterables to tuples if they are not tuples 265 # already 266 if type(bodies) != tuple and bodies is not None: 267 bodies = tuple(bodies) 268 if type(loads) != tuple and loads is not None: 269 loads = tuple(loads) 270 self._bodies = bodies 271 self._loads = loads 272 273 @property 274 def coordinates(self): 275 """Returns the column matrix of the generalized coordinates""" 276 if self._coordinates is None: 277 raise AttributeError("The coordinates were not specified.") 278 else: 279 return self._coordinates 280 281 @property 282 def speeds(self): 283 """Returns the column matrix of generalized speeds""" 284 if self._speeds is None: 285 raise AttributeError("The speeds were not specified.") 286 else: 287 return self._speeds 288 289 @property 290 def states(self): 291 """Returns the column matrix of the state variables""" 292 return self._states 293 294 @property 295 def alg_con(self): 296 """Returns a list with the indices of the rows containing algebraic 297 constraints in the combined form of the equations of motion""" 298 return self._alg_con 299 300 @property 301 def dyn_implicit_mat(self): 302 """Returns the matrix, M, corresponding to the dynamic equations in 303 implicit form, M x' = F, where the kinematical equations are not 304 included""" 305 if self._dyn_implicit_mat is None: 306 raise AttributeError("dyn_implicit_mat is not specified for " 307 "equations of motion form [1] or [2].") 308 else: 309 return self._dyn_implicit_mat 310 311 @property 312 def dyn_implicit_rhs(self): 313 """Returns the column matrix, F, corresponding to the dynamic equations 314 in implicit form, M x' = F, where the kinematical equations are not 315 included""" 316 if self._dyn_implicit_rhs is None: 317 raise AttributeError("dyn_implicit_rhs is not specified for " 318 "equations of motion form [1] or [2].") 319 else: 320 return self._dyn_implicit_rhs 321 322 @property 323 def comb_implicit_mat(self): 324 """Returns the matrix, M, corresponding to the equations of motion in 325 implicit form (form [2]), M x' = F, where the kinematical equations are 326 included""" 327 if self._comb_implicit_mat is None: 328 if self._dyn_implicit_mat is not None: 329 num_kin_eqns = len(self._kin_explicit_rhs) 330 num_dyn_eqns = len(self._dyn_implicit_rhs) 331 zeros1 = zeros(num_kin_eqns, num_dyn_eqns) 332 zeros2 = zeros(num_dyn_eqns, num_kin_eqns) 333 inter1 = eye(num_kin_eqns).row_join(zeros1) 334 inter2 = zeros2.row_join(self._dyn_implicit_mat) 335 self._comb_implicit_mat = inter1.col_join(inter2) 336 return self._comb_implicit_mat 337 else: 338 raise AttributeError("comb_implicit_mat is not specified for " 339 "equations of motion form [1].") 340 else: 341 return self._comb_implicit_mat 342 343 @property 344 def comb_implicit_rhs(self): 345 """Returns the column matrix, F, corresponding to the equations of 346 motion in implicit form (form [2]), M x' = F, where the kinematical 347 equations are included""" 348 if self._comb_implicit_rhs is None: 349 if self._dyn_implicit_rhs is not None: 350 kin_inter = self._kin_explicit_rhs 351 dyn_inter = self._dyn_implicit_rhs 352 self._comb_implicit_rhs = kin_inter.col_join(dyn_inter) 353 return self._comb_implicit_rhs 354 else: 355 raise AttributeError("comb_implicit_mat is not specified for " 356 "equations of motion in form [1].") 357 else: 358 return self._comb_implicit_rhs 359 360 def compute_explicit_form(self): 361 """If the explicit right hand side of the combined equations of motion 362 is to provided upon initialization, this method will calculate it. This 363 calculation can potentially take awhile to compute.""" 364 if self._comb_explicit_rhs is not None: 365 raise AttributeError("comb_explicit_rhs is already formed.") 366 367 inter1 = getattr(self, 'kin_explicit_rhs', None) 368 if inter1 is not None: 369 inter2 = self._dyn_implicit_mat.LUsolve(self._dyn_implicit_rhs) 370 out = inter1.col_join(inter2) 371 else: 372 out = self._comb_implicit_mat.LUsolve(self._comb_implicit_rhs) 373 374 self._comb_explicit_rhs = out 375 376 @property 377 def comb_explicit_rhs(self): 378 """Returns the right hand side of the equations of motion in explicit 379 form, x' = F, where the kinematical equations are included""" 380 if self._comb_explicit_rhs is None: 381 raise AttributeError("Please run .combute_explicit_form before " 382 "attempting to access comb_explicit_rhs.") 383 else: 384 return self._comb_explicit_rhs 385 386 @property 387 def kin_explicit_rhs(self): 388 """Returns the right hand side of the kinematical equations in explicit 389 form, q' = G""" 390 if self._kin_explicit_rhs is None: 391 raise AttributeError("kin_explicit_rhs is not specified for " 392 "equations of motion form [1] or [2].") 393 else: 394 return self._kin_explicit_rhs 395 396 def dynamic_symbols(self): 397 """Returns a column matrix containing all of the symbols in the system 398 that depend on time""" 399 # Create a list of all of the expressions in the equations of motion 400 if self._comb_explicit_rhs is None: 401 eom_expressions = (self.comb_implicit_mat[:] + 402 self.comb_implicit_rhs[:]) 403 else: 404 eom_expressions = (self._comb_explicit_rhs[:]) 405 406 functions_of_time = set() 407 for expr in eom_expressions: 408 functions_of_time = functions_of_time.union( 409 find_dynamicsymbols(expr)) 410 functions_of_time = functions_of_time.union(self._states) 411 412 return tuple(functions_of_time) 413 414 def constant_symbols(self): 415 """Returns a column matrix containing all of the symbols in the system 416 that do not depend on time""" 417 # Create a list of all of the expressions in the equations of motion 418 if self._comb_explicit_rhs is None: 419 eom_expressions = (self.comb_implicit_mat[:] + 420 self.comb_implicit_rhs[:]) 421 else: 422 eom_expressions = (self._comb_explicit_rhs[:]) 423 424 constants = set() 425 for expr in eom_expressions: 426 constants = constants.union(expr.free_symbols) 427 constants.remove(dynamicsymbols._t) 428 429 return tuple(constants) 430 431 @property 432 def bodies(self): 433 """Returns the bodies in the system""" 434 if self._bodies is None: 435 raise AttributeError("bodies were not specified for the system.") 436 else: 437 return self._bodies 438 439 @property 440 def loads(self): 441 """Returns the loads in the system""" 442 if self._loads is None: 443 raise AttributeError("loads were not specified for the system.") 444 else: 445 return self._loads 446