# -*- coding: utf-8 -*- """ Define algebra functions for package MBsysPy. Summary ------- Defines the usefull functions that are not specific to a MBS analysis. Mainly calls corresponding numpy function but ignoring first element of array. """ # Author : Robotran Team # (c) Universite catholique de Louvain, 2020 import numpy as np from math import cos, sin def norm(v, size_4=True): """Compute the norm of a vector, starting with index 1. Parameters ---------- v : numpy.ndarray or list 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 If v is not a numpy.ndarray or a list. Returns ------- float Norm of vector v starting at index 1. """ v, mat = __get_index_0_matrix__(v, size_4) if mat: raise ValueError('Vector must be passed as argument for norm function.') return np.linalg.norm(v) def normalize(v, vres=None, size_4=True): """Normalize vector and return a copy, starting with index 1. Parameters ---------- v : numpy.ndarray or list Vector with first element unused. vres : list or numpy.ndarray, optional Vector containing the result if not None. default: None size_4 : bool, optional True if v has a size of 4. default: True. Raises ------ ZeroDivisionError If the norm of v is equal to zero Returns ------- v_norm : numpy.ndarray Copy of normalized vector v starting at index 1. """ n = norm(v, size_4) if n == 0: raise ZeroDivisionError('The norm of v is equal to zero') else: 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 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 ---------- v1 : list or numpy.ndarray Vector with first element unused. v2 : list or numpy.ndarray Vector with first element unused. size_4 : bool, optional True if vectors have a size of 4. default: True. Raises ------ ValueError If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError If v1 or v2 is not a numpy.ndarray or a list. Returns ------- float Scalar product between v1 and v2. """ v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) if mat1 or mat2: raise ValueError('Vectors must be passed as argument for scalar product') return np.dot(v1, v2) def vector_sum(v1, v2, vres=None, size_4=True): """Compute and return the sum of 2 vectors with first index is 1. The vectors have unused index 0. Parameters ---------- v1 : list or numpy.ndarray Vector with first element unused. v2 : list or numpy.ndarray Vector with first element unused. vres : list or numpy.ndarray, optional Vector containing the result if not None. default: None size_4 : bool, optional True if vectors have a size of 4. default: True. Raises ------ ValueError If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError If v1 or v2 is not a numpy.ndarray or a list. Returns ------- v_sum : numpy.ndarray Sum of v1 and v2. """ v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) 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 else: if (vres.shape)[0] == 1: vres[0, 1:] = v_sum else: vres[1:, 0] = v_sum else: v_sum = np.append(v_sum.shape[0], v_sum) return v_sum 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 ---------- v1 : list or numpy.ndarray Vector with first element unused. v2 : list or numpy.ndarray Vector with first element unused. vres : list or numpy.ndarray, optional Vector containing the result if not None. default: None size_4 : bool, optional True if vectors have a size of 4. default: True. Raises ------ ValueError If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError If v1 or v2 is not a numpy.ndarray or a list. Returns ------- v_diff : numpy.ndarray Substraction of v1 and v2 (v1 - v2). """ v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) 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 else: if (vres.shape)[0] == 1: vres[0, 1:] = v_diff else: vres[1:, 0] = v_diff else: v_diff = np.append(v_diff.shape[0], v_diff) return v_diff 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 ---------- v1 : list or numpy.ndarray Vector with first element unused. v2 : list or numpy.ndarray Vector with first element unused. vres : list or numpy.ndarray, optional Vector containing the result if not None. default: None size_4 : bool, optional True if vectors have a size of 4. default: True. Raises ------ ValueError If v1 or v2 is not a 1-dim array of size 4 if size_4 is True. TypeError If v1 or v2 is not a numpy.ndarray or a list. Returns ------- v_cross : numpy.ndarray Cross product between v1 and v2 (v1 x v2). """ v1, mat1 = __get_index_0_matrix__(v1, size_4) v2, mat2 = __get_index_0_matrix__(v2, size_4) 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 else: if (vres.shape)[0] == 1: vres[0, 1:] = v_cross else: vres[1:, 0] = v_cross else: v_cross = np.append(v_cross.shape[0], v_cross) return v_cross def matrix_product(M1, M2, Mres=None, size_4=True): """Compute the product of 2 matrices. The first index is 1. The second parameter can be a vector. Matrices and vector have unused index 0. Parameters ---------- M1 : list or numpy.ndarray Matrix with first line and row unused, will be multiplied by M2. M2 : list or numpy.ndarray Matrix with first line and row unused. It could also be a vector with first element unused Mres : list or numpy.ndarray, optional 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 size_4 : bool, optional True if vectors have a size of 4 and matrices are 4x4. default: True. Raises ------ ValueError If M1 or M2 is not an array of size 4 if size_4 is True. TypeError If M1 or M2 is not a numpy.ndarray or a list. Returns ------- M_prod : numpy.ndarray Matrix product between M1 and M2 (M . M2) if Mres is None. """ M1, mat1 = __get_index_0_matrix__(M1, size_4) M2, mat2 = __get_index_0_matrix__(M2, size_4) 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 else: if (Mres.shape)[0] == 1: Mres[0, 1:] = M_prod else: 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)) else: M_prod = np.append(M_prod.shape[0], M_prod) return M_prod def __get_index_0_matrix__(M, size_4): """Remove first index and check if right dimensions. Parameters ---------- M : list or numpy.ndarray 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 If M is not a numpy.ndarray or a list. Returns ------- M1 : numpy.ndarray M without first index. mat : bool True if M1 is a matrix. """ mat = True if isinstance(M, (list, np.ndarray)): 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 else: 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 else: raise TypeError('Matrices must be numpy.ndarray or list') def rotation_matrix(rot_type, angle, Rres=None): """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. Parameters ---------- rot_type : int Integer for axis selection 1: rotation along x-axis. 2: rotation along y-axis. 3: rotation along z-axis. angle : float Rotation angle expressed in radian. Mres : numpy.ndarray, optional Matrix containing the result if not None. default: None Raises ------ ValueError If rot_type different from 1, 2 or 3. Returns ------- Rres : numpy.ndarray Rotation matrix. """ 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) if rot_type == 1: Rres[1, 1] = 1. Rres[2, 2] = c Rres[3, 2] = -s Rres[2, 3] = s Rres[3, 3] = c elif rot_type == 2: Rres[2, 2] = 1. Rres[1, 1] = c Rres[3, 1] = s Rres[1, 3] = -s Rres[3, 3] = c elif rot_type == 3: Rres[3, 3] = 1. Rres[1, 1] = c Rres[2, 1] = -s Rres[1, 2] = s Rres[2, 2] = c return Rres