mbs_algebra.py 11.6 KB
 Louis Beauloye committed Aug 12, 2020 1 2 3 4 5 6 ``````# -*- coding: utf-8 -*- """ Define algebra functions for package MBsysPy. Summary ------- `````` Olivier Lantsoght committed Aug 14, 2020 7 8 ``````Defines the usefull functions that are not specific to a MBS analysis. Mainly calls corresponding numpy function but ignoring first element of array. `````` Louis Beauloye committed Aug 12, 2020 9 10 11 12 ``````""" # Author : Robotran Team # (c) Universite catholique de Louvain, 2020 import numpy as np `````` Olivier Lantsoght committed Jan 29, 2021 13 ``````from math import cos, sin `````` Louis Beauloye committed Aug 12, 2020 14 15 `````` `````` Louis Beauloye committed Aug 13, 2020 16 17 18 19 20 ``````def norm(v, size_4=True): """Compute the norm of a vector, starting with index 1. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 21 `````` v : numpy.ndarray or list `````` Louis Beauloye committed Aug 13, 2020 22 23 24 25 26 27 28 29 30 31 `````` Vector with first element unused. size_4 : bool, optional True if vectors have a size of 4. default: True. Raises ------ ValueError If v is not a 1-dim array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 32 `````` If v is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 13, 2020 33 34 35 36 37 38 39 `````` Returns ------- float Norm of vector v starting at index 1. """ `````` Olivier Lantsoght committed Aug 14, 2020 40 `````` v, mat = __get_index_0_matrix__(v, size_4) `````` Louis Beauloye committed Aug 13, 2020 41 `````` if mat: `````` Olivier Lantsoght committed Aug 14, 2020 42 43 `````` raise ValueError('Vector must be passed as argument for norm function.') return np.linalg.norm(v) `````` Louis Beauloye committed Aug 13, 2020 44 45 `````` `````` Louis Beauloye committed Aug 13, 2020 46 47 48 ``````def normalize(v, vres=None, size_4=True): """Normalize vector and return a copy, starting with index 1. `````` Louis Beauloye committed Aug 13, 2020 49 50 `````` Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 51 `````` v : numpy.ndarray or list `````` Louis Beauloye committed Aug 13, 2020 52 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 53 `````` vres : list or numpy.ndarray, optional `````` Louis Beauloye committed Aug 13, 2020 54 55 `````` Vector containing the result if not None. default: None `````` Louis Beauloye committed Aug 13, 2020 56 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 57 `````` True if v has a size of 4. `````` Louis Beauloye committed Aug 13, 2020 58 59 60 61 62 63 64 65 66 `````` default: True. Raises ------ ZeroDivisionError If the norm of v is equal to zero Returns ------- `````` Louis Beauloye committed Aug 14, 2020 67 `````` v_norm : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 68 `````` Copy of normalized vector v starting at index 1. `````` Louis Beauloye committed Aug 13, 2020 69 70 71 72 73 74 `````` """ n = norm(v, size_4) if n == 0: raise ZeroDivisionError('The norm of v is equal to zero') else: `````` Louis Beauloye committed Aug 13, 2020 75 76 77 78 79 80 81 82 83 `````` if vres is not None: if len(np.shape(vres)) == 1: vres[1:] = v[1:] / n else: vres[0, 1:] = v[1:] / n else: v_norm = np.array(v, dtype=float) v_norm[1:] = v_norm[1:] / n return v_norm `````` Louis Beauloye committed Aug 13, 2020 84 85 `````` `````` Louis Beauloye committed Aug 12, 2020 86 87 88 89 90 91 92 93 ``````def scalar_product(v1, v2, size_4=True): """Compute and return the scalar product of 2 vectors with first index is 1. The vectors have unused index 0. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 94 `````` v1 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 95 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 96 `````` v2 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 97 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 12, 2020 98 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 99 100 `````` True if vectors have a size of 4. default: True. `````` Louis Beauloye committed Aug 12, 2020 101 102 103 104 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 105 106 `````` If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 107 `````` If v1 or v2 is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 12, 2020 108 109 110 111 `````` Returns ------- float `````` Louis Beauloye committed Aug 13, 2020 112 `````` Scalar product between v1 and v2. `````` Louis Beauloye committed Aug 12, 2020 113 114 `````` """ `````` Olivier Lantsoght committed Aug 14, 2020 115 116 `````` v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) `````` Louis Beauloye committed Aug 13, 2020 117 118 119 `````` if mat1 or mat2: raise ValueError('Vectors must be passed as argument for scalar product') return np.dot(v1, v2) `````` Louis Beauloye committed Aug 12, 2020 120 121 `````` `````` Louis Beauloye committed Aug 13, 2020 122 ``````def vector_sum(v1, v2, vres=None, size_4=True): `````` Louis Beauloye committed Aug 12, 2020 123 124 125 126 127 128 129 `````` """Compute and return the sum of 2 vectors with first index is 1. The vectors have unused index 0. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 130 `````` v1 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 131 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 132 `````` v2 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 133 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 134 `````` vres : list or numpy.ndarray, optional `````` Louis Beauloye committed Aug 13, 2020 135 136 `````` Vector containing the result if not None. default: None `````` Louis Beauloye committed Aug 12, 2020 137 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 138 139 `````` True if vectors have a size of 4. default: True. `````` Louis Beauloye committed Aug 12, 2020 140 141 142 143 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 144 145 `````` If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 146 `````` If v1 or v2 is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 12, 2020 147 148 149 `````` Returns ------- `````` Louis Beauloye committed Aug 14, 2020 150 `````` v_sum : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 151 `````` Sum of v1 and v2. `````` Louis Beauloye committed Aug 12, 2020 152 153 `````` """ `````` Olivier Lantsoght committed Aug 14, 2020 154 155 `````` v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) `````` Louis Beauloye committed Aug 13, 2020 156 157 158 159 160 161 `````` if mat1 or mat2: raise ValueError('Vectors must be passed as argument for addition') v_sum = v1 + v2 if vres is not None: if len(np.shape(vres)) == 1: vres[1:] = v_sum `````` Louis Beauloye committed Aug 12, 2020 162 `````` else: `````` Louis Beauloye committed Aug 13, 2020 163 164 `````` if (vres.shape)[0] == 1: vres[0, 1:] = v_sum `````` Louis Beauloye committed Aug 12, 2020 165 `````` else: `````` Louis Beauloye committed Aug 13, 2020 166 `````` vres[1:, 0] = v_sum `````` Louis Beauloye committed Aug 12, 2020 167 `````` else: `````` Louis Beauloye committed Aug 20, 2020 168 `````` v_sum = np.append(v_sum.shape[0], v_sum) `````` Louis Beauloye committed Aug 13, 2020 169 `````` return v_sum `````` Louis Beauloye committed Aug 12, 2020 170 171 172 173 174 175 176 177 178 179 `````` def vector_diff(v1, v2, vres=None, size_4=True): """Compute and return the substraction of 2 vectors with first index is 1. The vectors have unused index 0. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 180 `````` v1 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 181 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 182 `````` v2 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 183 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 184 `````` vres : list or numpy.ndarray, optional `````` Louis Beauloye committed Aug 13, 2020 185 186 `````` Vector containing the result if not None. default: None `````` Louis Beauloye committed Aug 12, 2020 187 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 188 189 `````` True if vectors have a size of 4. default: True. `````` Louis Beauloye committed Aug 12, 2020 190 191 192 193 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 194 195 `````` If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 196 `````` If v1 or v2 is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 12, 2020 197 198 199 `````` Returns ------- `````` Louis Beauloye committed Aug 14, 2020 200 `````` v_diff : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 201 `````` Substraction of v1 and v2 (v1 - v2). `````` Louis Beauloye committed Aug 12, 2020 202 203 `````` """ `````` Olivier Lantsoght committed Aug 14, 2020 204 205 `````` v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) `````` Louis Beauloye committed Aug 13, 2020 206 207 208 209 210 211 `````` if mat1 or mat2: raise ValueError('Vectors must be passed as argument for substraction') v_diff = v1 - v2 if vres is not None: if len(np.shape(vres)) == 1: vres[1:] = v_diff `````` Louis Beauloye committed Aug 12, 2020 212 `````` else: `````` Louis Beauloye committed Aug 13, 2020 213 214 `````` if (vres.shape)[0] == 1: vres[0, 1:] = v_diff `````` Louis Beauloye committed Aug 12, 2020 215 `````` else: `````` Louis Beauloye committed Aug 13, 2020 216 `````` vres[1:, 0] = v_diff `````` Louis Beauloye committed Aug 12, 2020 217 `````` else: `````` Louis Beauloye committed Aug 20, 2020 218 `````` v_diff = np.append(v_diff.shape[0], v_diff) `````` Louis Beauloye committed Aug 13, 2020 219 `````` return v_diff `````` Louis Beauloye committed Aug 12, 2020 220 221 222 223 224 225 226 227 228 229 `````` def cross_product(v1, v2, vres=None, size_4=True): """Compute and return the cross product of 2 vectors with first index is 1. The vectors have unused index 0. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 230 `````` v1 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 231 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 232 `````` v2 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 233 `````` Vector with first element unused. `````` Louis Beauloye committed Aug 14, 2020 234 `````` vres : list or numpy.ndarray, optional `````` Louis Beauloye committed Aug 13, 2020 235 236 `````` Vector containing the result if not None. default: None `````` Louis Beauloye committed Aug 12, 2020 237 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 238 239 `````` True if vectors have a size of 4. default: True. `````` Louis Beauloye committed Aug 12, 2020 240 241 242 243 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 244 245 `````` If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 246 `````` If v1 or v2 is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 12, 2020 247 248 249 `````` Returns ------- `````` Louis Beauloye committed Aug 14, 2020 250 `````` v_cross : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 251 `````` Cross product between v1 and v2 (v1 x v2). `````` Louis Beauloye committed Aug 12, 2020 252 253 `````` """ `````` Olivier Lantsoght committed Aug 14, 2020 254 255 `````` v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) `````` Louis Beauloye committed Aug 13, 2020 256 257 258 259 260 261 `````` if mat1 or mat2: raise ValueError('Vectors must be passed as argument for cross product') v_cross = np.cross(v1, v2) if vres is not None: if len(np.shape(vres)) == 1: vres[1:] = v_cross `````` Louis Beauloye committed Aug 12, 2020 262 `````` else: `````` Louis Beauloye committed Aug 13, 2020 263 264 `````` if (vres.shape)[0] == 1: vres[0, 1:] = v_cross `````` Louis Beauloye committed Aug 12, 2020 265 `````` else: `````` Louis Beauloye committed Aug 13, 2020 266 `````` vres[1:, 0] = v_cross `````` Louis Beauloye committed Aug 12, 2020 267 `````` else: `````` Louis Beauloye committed Aug 20, 2020 268 `````` v_cross = np.append(v_cross.shape[0], v_cross) `````` Louis Beauloye committed Aug 13, 2020 269 `````` return v_cross `````` Louis Beauloye committed Aug 12, 2020 270 271 `````` `````` Louis Beauloye committed Aug 13, 2020 272 273 ``````def matrix_product(M1, M2, Mres=None, size_4=True): """Compute the product of 2 matrices. The first index is 1. `````` Louis Beauloye committed Aug 12, 2020 274 `````` `````` Louis Beauloye committed Aug 13, 2020 275 276 `````` The second parameter can be a vector. Matrices and vector have unused index 0. `````` Louis Beauloye committed Aug 12, 2020 277 278 279 280 `````` Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 281 `````` M1 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 282 `````` Matrix with first line and row unused, will be multiplied by M2. `````` Louis Beauloye committed Aug 14, 2020 283 `````` M2 : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 284 285 `````` Matrix with first line and row unused. It could also be a vector with first element unused `````` Louis Beauloye committed Aug 14, 2020 286 `````` Mres : list or numpy.ndarray, optional `````` Louis Beauloye committed Aug 13, 2020 287 288 289 290 `````` Matrice containing the result if not None. It must have the right dimensions. If M2 is a vector, Mres also has to be a vector default: None `````` Louis Beauloye committed Aug 12, 2020 291 `````` size_4 : bool, optional `````` Louis Beauloye committed Aug 13, 2020 292 293 `````` True if vectors have a size of 4 and matrices are 4x4. default: True. `````` Louis Beauloye committed Aug 12, 2020 294 295 296 297 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 298 299 `````` If M1 or M2 is not an array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 300 `````` If M1 or M2 is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 12, 2020 301 302 303 `````` Returns ------- `````` Louis Beauloye committed Aug 14, 2020 304 `````` M_prod : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 305 `````` Matrix product between M1 and M2 (M . M2) if Mres is None. `````` Louis Beauloye committed Aug 12, 2020 306 307 `````` """ `````` Olivier Lantsoght committed Aug 14, 2020 308 309 `````` M1, mat1 = __get_index_0_matrix__(M1, size_4) M2, mat2 = __get_index_0_matrix__(M2, size_4) `````` Louis Beauloye committed Aug 13, 2020 310 311 312 313 314 315 316 `````` M_prod = np.dot(M1, M2) if Mres is not None: if mat2: Mres[1:, 1:] = M_prod else: if len(np.shape(Mres)) == 1: Mres[1:] = M_prod `````` Louis Beauloye committed Aug 13, 2020 317 `````` else: `````` Louis Beauloye committed Aug 13, 2020 318 319 `````` if (Mres.shape)[0] == 1: Mres[0, 1:] = M_prod `````` Louis Beauloye committed Aug 12, 2020 320 `````` else: `````` Louis Beauloye committed Aug 13, 2020 321 322 323 324 325 `````` Mres[1:, 0] = M_prod else: if mat2: M_prod = np.hstack((np.ones((M_prod.shape[0], 1)) * M_prod.shape[0], M_prod)) M_prod = np.vstack((np.ones((1, M_prod.shape[1])) * M_prod.shape[0], M_prod)) `````` Louis Beauloye committed Aug 12, 2020 326 `````` else: `````` Louis Beauloye committed Aug 20, 2020 327 `````` M_prod = np.append(M_prod.shape[0], M_prod) `````` Louis Beauloye committed Aug 13, 2020 328 329 330 331 `````` return M_prod `````` Olivier Lantsoght committed Aug 14, 2020 332 ``````def __get_index_0_matrix__(M, size_4): `````` Louis Beauloye committed Aug 13, 2020 333 334 335 336 `````` """Remove first index and check if right dimensions. Parameters ---------- `````` Louis Beauloye committed Aug 14, 2020 337 `````` M : list or numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 338 339 340 341 342 343 344 345 346 347 `````` Matrix or vector. size_4 : bool, optional True if vectors have a size of 4 and matrices are 4x4. default: True. Raises ------ ValueError If M is not an array of size 4 if size_4 is True. TypeError `````` Louis Beauloye committed Aug 14, 2020 348 `````` If M is not a numpy.ndarray or a list. `````` Louis Beauloye committed Aug 13, 2020 349 350 351 `````` Returns ------- `````` Louis Beauloye committed Aug 14, 2020 352 `````` M1 : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 353 354 355 356 357 358 `````` M without first index. mat : bool True if M1 is a matrix. """ mat = True `````` Louis Beauloye committed Aug 14, 2020 359 `````` if isinstance(M, (list, np.ndarray)): `````` Louis Beauloye committed Aug 13, 2020 360 361 362 363 364 365 366 367 368 `````` if len(np.shape(M)) == 1: M = np.array([M], dtype=float) if len(np.shape(M)) == 2: if (M.shape)[0] == 1: M1 = np.array(M[0, 1:], dtype=float) mat = False elif (M.shape)[1] == 1: M1 = np.array(M[1:, 0], dtype=float) mat = False `````` Louis Beauloye committed Aug 12, 2020 369 `````` else: `````` Louis Beauloye committed Aug 13, 2020 370 371 372 373 374 375 376 `````` M1 = np.array(M[1:, 1:], dtype=float) else: raise ValueError('Matrices with more than 2 dimensions are not handled') if size_4 and (not np.all(np.array(M1.shape) == 3)): raise ValueError('Matrices must be arrays of size 4 if "size_4" is True') return M1, mat `````` Louis Beauloye committed Aug 12, 2020 377 `````` else: `````` Louis Beauloye committed Aug 14, 2020 378 `````` raise TypeError('Matrices must be numpy.ndarray or list') `````` Louis Beauloye committed Aug 12, 2020 379 380 381 `````` def rotation_matrix(rot_type, angle, Rres=None): `````` Olivier Lantsoght committed Jan 29, 2021 382 383 384 385 `````` """Compute the rotation matrix for a specified angle around a specific axis. The rotation matrices uses the MBsysC convention wich means that the first index is 1. `````` Louis Beauloye committed Aug 12, 2020 386 387 388 389 `````` Parameters ---------- rot_type : int `````` Louis Beauloye committed Aug 13, 2020 390 `````` Integer for axis selection `````` Louis Beauloye committed Aug 12, 2020 391 392 393 394 `````` 1: rotation along x-axis. 2: rotation along y-axis. 3: rotation along z-axis. angle : float `````` Louis Beauloye committed Aug 13, 2020 395 `````` Rotation angle expressed in radian. `````` Olivier Lantsoght committed Jan 29, 2021 396 397 `````` Mres : numpy.ndarray, optional Matrix containing the result if not None. `````` Louis Beauloye committed Aug 13, 2020 398 `````` default: None `````` Louis Beauloye committed Aug 12, 2020 399 400 401 `````` Raises ------ ValueError `````` Louis Beauloye committed Aug 13, 2020 402 `````` If rot_type different from 1, 2 or 3. `````` Louis Beauloye committed Aug 12, 2020 403 404 405 `````` Returns ------- `````` Olivier Lantsoght committed Jan 29, 2021 406 `````` Rres : numpy.ndarray `````` Louis Beauloye committed Aug 13, 2020 407 `````` Rotation matrix. `````` Louis Beauloye committed Aug 12, 2020 408 409 `````` """ `````` Olivier Lantsoght committed Jan 29, 2021 410 411 412 413 414 415 416 417 418 419 420 `````` if rot_type not in [1, 2, 3]: raise ValueError('{:} is not a valid rotation type'.format(rot_type)) if Rres is None: Rres = np.zeros((4, 4)) Rres[:, 0] = 3. else: Rres[1:, 1:] = 0. c = cos(angle) s = sin(angle) `````` Louis Beauloye committed Aug 12, 2020 421 422 `````` if rot_type == 1: `````` Olivier Lantsoght committed Jan 29, 2021 423 424 425 426 427 `````` Rres[1, 1] = 1. Rres[2, 2] = c Rres[3, 2] = -s Rres[2, 3] = s Rres[3, 3] = c `````` Louis Beauloye committed Aug 12, 2020 428 `````` elif rot_type == 2: `````` Olivier Lantsoght committed Jan 29, 2021 429 430 431 432 433 `````` Rres[2, 2] = 1. Rres[1, 1] = c Rres[3, 1] = s Rres[1, 3] = -s Rres[3, 3] = c `````` Louis Beauloye committed Aug 12, 2020 434 `````` elif rot_type == 3: `````` Olivier Lantsoght committed Jan 29, 2021 435 436 437 438 439 440 441 `````` Rres[3, 3] = 1. Rres[1, 1] = c Rres[2, 1] = -s Rres[1, 2] = s Rres[2, 2] = c return Rres``````