Commit 19bfba3d authored by Nicolas Docquier's avatar Nicolas Docquier
Browse files

Merge branch 'MBsysPy_algebra' into 'dev'

MBsysPy matrix operations

See merge request robotran/mbsysc!405
parents 3cc01b61 81b88912
......@@ -10,375 +10,366 @@ 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.
def norm(v, index_1=True):
"""Compute the norm of a vector.
The vector is assumed to store the values in the indices [1:]. If not it
must be specified (see parameter '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.
v : numpy.ndarray
Vector with first element (index_1) unused by default.
index_1 : bool, optional
If False, the value v[0] is included in the computation.
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)
return np.linalg.norm(v[index_1:])
def normalize(v, vres=None, index_1=True):
"""Normalize the provided vector.
def normalize(v, vres=None, size_4=True):
"""Normalize vector and return a copy, starting with index 1.
The vector is assumed to store the values in the indices [1:]. If not it
must be specified (see parameter '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.
v : numpy.ndarray
Vector to be normalized with first element (index_1) unused by default.
vres : numpy.ndarray, optional
Vector to store the normalized vector. It is allocated if not provided,
the used indices are the same as the input vector v.
default: None
size_4 : bool, optional
True if v has a size of 4.
index_1 : bool, optional
If False, the value v[0] is included in the computation.
default: True.
Raises
------
ZeroDivisionError
If the norm of v is equal to zero
If the norm of v is equal to zero.
Returns
-------
v_norm : numpy.ndarray
Copy of normalized vector v starting at index 1.
vres : numpy.ndarray
The normalized vector v, the used indices are the same as the input
vector v.
"""
n = norm(v, size_4)
n = norm(v, index_1)
if n == 0:
raise ZeroDivisionError('The norm of v is equal to zero')
if vres is None:
vres = np.array(v)
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
vres[:] = v[:]
vres[index_1:] = vres[index_1:] / n
return vres
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.
def scalar_product(v1, v2, index_1=True):
"""Compute and return the scalar product of 2 vectors.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
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.
v1 : numpy.ndarray
First vector of the product with first element (index_1) unused by default.
v2 : numpy.ndarray
Second vector of the product with first element (index_1) unused by default.
index_1 : bool, optional
If False, the value v1[0] and v2[0] are included in the computation.
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)
return np.dot(v1[index_1:], v2[index_1:])
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.
def vector_sum(v1, v2, vres=None, index_1=True):
"""Compute and return the sum of 2 vectors.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
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.
v1 : numpy.ndarray
First vector of the addition with first element (index_1) unused by default.
v2 : numpy.ndarray
Second vector of the addition with first element (index_1) unused by default.
vres : numpy.ndarray, optional
Vector to store the result of v1+v2. It is allocated if not provided,
the used indices are the same as the input vector v1 and v2.
default: None
size_4 : bool, optional
True if vectors have a size of 4.
index_1 : bool, optional
If False, the value v1[0] and v2[0] are included in the computation.
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.
vres : numpy.ndarray
Sum of v1 and v2 (v1 + v2), the used indices are the same as the input
vectors.
"""
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
if vres is None:
vres = np.zeros(3 + index_1)
vres[index_1:] = v1[index_1:] + v2[index_1:]
def vector_diff(v1, v2, vres=None, size_4=True):
"""Compute and return the substraction of 2 vectors with first index is 1.
return vres
The vectors have unused index 0.
def vector_diff(v1, v2, vres=None, index_1=True):
"""Compute and return the substraction of 2 vectors.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
Parameters
----------
v1 : list or numpy.ndarray
Vector with first element unused.
v1 : numpy.ndarray
First vector of the substraction with first element (index_1) unused
by default.
v2 : list or numpy.ndarray
Vector with first element unused.
vres : list or numpy.ndarray, optional
Vector containing the result if not None.
Vector to be substracted with first element (index_1) unused by default.
vres : numpy.ndarray, optional
Vector to store the result of v1-v2. It is allocated if not provided,
the used indices are the same as the input vector v1 and v2.
default: None
size_4 : bool, optional
True if vectors have a size of 4.
index_1 : bool, optional
If False, the value v1[0] and v2[0] are included in the computation.
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).
vres : numpy.ndarray
Substraction of v1 and v2 (v1 - v2), the used indices are the same as
the input vectors.
"""
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
if vres is None:
vres = np.zeros(3 + index_1)
vres[index_1:] = v1[index_1:] - v2[index_1:]
return vres
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.
def cross_product(v1, v2, vres=None, index_1=True):
"""Compute and return the cross product of 2 vectors of 3 components.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
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.
v1 : numpy.ndarray
First vector of the cross product with first element (index_1) unused
by default. The number of (used) component must be 3.
v2 : numpy.ndarray
Second vector of the cross product with first element (index_1) unused
by default. The number of (used) component must be 3.
vres : numpy.ndarray, optional
Vector to store the result if not None. It is allocated if not provided,
the used indices are the same as the input vector v1 and v2.
default: None
size_4 : bool, optional
True if vectors have a size of 4.
index_1 : bool, optional
If False, the value v1[0] and v2[0] are included in the computation.
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).
vres : numpy.ndarray
Cross product between v1 and v2 (v1 x v2), the used indices are the same as
the input vectors.
"""
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
Notes
-----
The function numpy.cross() is not used as it is not optimized for such small
vector.
"""
if vres is None:
vres = np.zeros(3 + index_1)
# For faster reading, terms are aligned.
# For comprenhension:
# index_1 alone is index [1]
# index_1 + 1 is index [2]
# index_1 + 2 is index [3]
vres[index_1 ] = v1[index_1 + 1] * v2[index_1 + 2] - v1[index_1 + 2] * v2[index_1 + 1]
vres[index_1 + 1] = v1[index_1 + 2] * v2[index_1 ] - v1[index_1 ] * v2[index_1 + 2]
vres[index_1 + 2] = v1[index_1 ] * v2[index_1 + 1] - v1[index_1 + 1] * v2[index_1 ]
# And the previous formulaes are the cross product of 2 vector of 3 elements, first element being in index [1].
def matrix_product(M1, M2, Mres=None, size_4=True):
"""Compute the product of 2 matrices. The first index is 1.
return vres
The second parameter can be a vector.
Matrices and vector have unused index 0.
def matrix_vector_product(matrix, vector):
"""Compute the product of a matrix and a vector with first index are 1.
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.
matrix : np.ndarray
Matrix of shape (4, 4), the first line and first row are disregarded.
The computation only consider matrix[1:4, 1:4].
vector : np.ndarray
Vector of shape (4, ), the first lelement is disregarded.
The computation only consider vector[1:4].
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
-------
vector_res : numpy.ndarray
Result of the matrix product. The shape of the vector is (4, ). The first
element is disregarded.
See Also
--------
matrix_product :
Slower but more versatile implementation of this functions. It accept both
vector or matrix as second parameter and both index 0 or index 1 array.
matrix_matrix_product :
Fasr implementation restricted to matrix product between 2 matrices with
first index unused.
Notes
-----
Globally the functions simply calls numpy.dot(matrix[1:4, 1:4], vector[1:4])
"""
vector_res = np.zeros(4)
vector_res[1:4] = np.dot(matrix[1:4, 1:4], vector[1:4])[:]
return vector_res
def matrix_matrix_product(matrix_1, matrix_2):
"""Compute the product of 2 matrices with first index is 1.
Parameters
----------
matrix_1 : np.ndarray
Matrix of shape (4, 4), the first line and first row are disregarded.
The computation only consider matrix_1[1:4, 1:4].
matrix_2 : np.ndarray
Matrix of shape (4, 4), the first line and first row are disregarded.
The computation only consider matrix_2[1:4, 1:4].
Returns
-------
M_prod : numpy.ndarray
Matrix product between M1 and M2 (M . M2) if Mres is None.
matrix_res : numpy.ndarray
Result of the matrix product. The shape of the matrix is (4, 4), the first
line and first row are disregarded.
See Also
--------
matrix_product :
Slower but more versatile implementation of this functions. It accept both
vector or matrix as second parameter and both index 0 or index 1 array.
matrix_vector_product :
Fast implementation restricted to matrix product between a matrix and
a vector both with first index unused.
Notes
-----
Globally the functions simply calls numpy.dot(matrix_1[1:4, 1:4], matrix_2[1:4, 1:4])
"""
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)
matrix_res = np.zeros((4, 4))
matrix_res[1:, 1:] = np.dot(matrix_1[1:4, 1:4], matrix_2[1:4, 1:4])[:, :]
return matrix_res
return M_prod
def matrix_product(M1, M2, res=None, index_1=True):
"""Compute the product of 2 matrices.
The vector are assumed to store the values in the indices [1:4] while the
matrices in [1:4, 1:4]. If not it must be specified (see parameter 'index_1'),
then the indices are [0:3].
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.
M1 : numpy.ndarray
Matrix to be multiplied by M2. The first line and row are unused by
default (parameter 'index_1').
M2 : numpy.ndarray
Second matrix or vector of the product. For a matrix, the first lines
and rows are unused by default (parameter 'index_1'). For a vector the
first element is unused by default (parameter 'index_1').
res : numpy.ndarray, optional
Matrice or vector to store the result if not None. It is allocated if
not provided, the used indices are the same as the input arrays.
If provided, it must have the right dimensions.
default: None
index_1 : bool, optional
If False, the components [0:3] are considered for the computation (instead
of [1:4]).
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.
If M1 is not a matrix.
Returns
-------
M1 : numpy.ndarray
M without first index.
mat : bool
True if M1 is a matrix.
res : numpy.ndarray
Matrix product between M1 and M2 (M1 . M2). The array is allocated if
it was not provided, the used indices are the same as the input arrays.
See Also
--------
matrix_matrix_product :
Faster implementation restricted to matrix product between 2 matrices with
first index unused.
matrix_vector_product :
Faster implementation restricted to matrix product between a matrix and
a vector both with first index unused.
Notes
-----
Due to the checks on input parameters this function is slow. Faster implementation,
each specific to a product (matrix with vector or matrix with matrix) exist.
"""
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)
if M1.ndim != 2:
raise ValueError('Parameter M1 must be a matrix')
if M2.ndim not in [1, 2]:
raise ValueError('Parameter M2 must be a vector or a matrix.')
if res is None:
size = index_1 + 3
if M2.ndim == 2:
res = np.zeros((size, size))
res[index_1:, index_1:] = np.dot(M1[index_1:size, index_1:size], M2[index_1:size, index_1:size])[:, :]
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')
res = np.zeros(size)
res[index_1:] = np.dot(M1[index_1:size, index_1:size], M2[index_1:size])[:]
return M1, mat
else:
raise TypeError('Matrices must be numpy.ndarray or list')
return res
def rotation_matrix(rot_type, angle, Rres=None):
"""Compute the rotation matrix for a specified angle around a specific axis. The first index is 1.
"""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.