Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
robotran
mbsysc
Commits
c1cfde85
Commit
c1cfde85
authored
Feb 01, 2021
by
Olivier Lantsoght
Browse files
[MBsysPy][algebra] size_4 removed for more explicit index_1
parent
3cf7d5c6
Changes
1
Hide whitespace changes
Inline
Side-by-side
MBsysC/mbs_interface/MBsysPy/mbs_algebra.py
View file @
c1cfde85
...
...
@@ -13,40 +13,44 @@ 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.
Returns
-------
float
Norm of vector v starting at index 1.
"""
return
np
.
linalg
.
norm
(
v
[
size_4
:])
return
np
.
linalg
.
norm
(
v
[
index_1
:])
def
normalize
(
v
,
vres
=
None
,
size_4
=
True
):
def
normalize
(
v
,
vres
=
None
,
index_1
=
True
):
"""Normalize the provided vector.
The shape of the vector differs according to size_4.
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
Vector to be normalized.
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.
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
...
...
@@ -57,10 +61,11 @@ def normalize(v, vres=None, size_4=True):
Returns
-------
vres : numpy.ndarray
The normalized vector v.
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'
)
...
...
@@ -69,24 +74,25 @@ def normalize(v, vres=None, size_4=True):
else
:
vres
[:]
=
v
[:]
vres
[
size_4
:]
=
vres
[
size_4
:]
/
n
vres
[
index_1
:]
=
vres
[
index_1
:]
/
n
return
vres
def
scalar_product
(
v1
,
v2
,
size_4
=
True
):
def
scalar_product
(
v1
,
v2
,
index_1
=
True
):
"""Compute and return the scalar product of 2 vectors.
The shape of the vector differs according to size_4.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
Parameters
----------
v1 : numpy.ndarray
First vector of the product.
First vector of the product
with first element (index_1) unused by default
.
v2 : numpy.ndarray
Second vector of the product.
size_4
: bool, optional
True if vectors have a size of 4
.
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.
Returns
...
...
@@ -94,98 +100,106 @@ def scalar_product(v1, v2, size_4=True):
float
Scalar product between v1 and v2.
"""
return
np
.
dot
(
v1
[
size_4
:],
v2
[
size_4
:])
return
np
.
dot
(
v1
[
index_1
:],
v2
[
index_1
:])
def
vector_sum
(
v1
,
v2
,
vres
=
None
,
size_4
=
True
):
def
vector_sum
(
v1
,
v2
,
vres
=
None
,
index_1
=
True
):
"""Compute and return the sum of 2 vectors.
The
shape of the vector differs according to size_4.
The
vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
Parameters
----------
v1 : numpy.ndarray
First vector of the addition.
First vector of the addition
with first element (index_1) unused by default
.
v2 : numpy.ndarray
Second vector of the addition.
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.
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.
Returns
-------
vres : numpy.ndarray
Sum of v1 and v2.
Sum of v1 and v2 (v1 + v2), the used indices are the same as the input
vectors.
"""
if
vres
is
None
:
vres
=
np
.
zeros
(
3
+
size_4
)
vres
=
np
.
zeros
(
3
+
index_1
)
vres
[
size_4
:]
=
v1
[
size_4
:]
+
v2
[
size_4
:]
vres
[
index_1
:]
=
v1
[
index_1
:]
+
v2
[
index_1
:]
return
vres
def
vector_diff
(
v1
,
v2
,
vres
=
None
,
size_4
=
True
):
def
vector_diff
(
v1
,
v2
,
vres
=
None
,
index_1
=
True
):
"""Compute and return the substraction of 2 vectors.
The shape of the vector differs according to size_4.
The vectors are assumed to store the values in the indices [1:]. If not it
must be specified (see parameter 'index_1').
Parameters
----------
v1 : numpy.ndarray
First vector of the substraction.
First vector of the substraction with first element (index_1) unused
by default.
v2 : list or numpy.ndarray
Vector to be substracted.
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.
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 (the first index being disregarded)
.
index_1
: bool, optional
If False, the value v1[0] and v2[0] are included in the computation
.
default: True.
Returns
-------
vres : numpy.ndarray
Substraction of v1 and v2 (v1 - v2).
Substraction of v1 and v2 (v1 - v2), the used indices are the same as
the input vectors.
"""
if
vres
is
None
:
vres
=
np
.
zeros
(
3
+
size_4
)
vres
=
np
.
zeros
(
3
+
index_1
)
vres
[
size_4
:]
=
v1
[
size_4
:]
-
v2
[
size_4
:]
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.
The shape of the vector differs according to size_4.
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 : numpy.ndarray
First vector of the cross product.
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.
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.
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.
Returns
-------
vres : numpy.ndarray
Cross product between v1 and v2 (v1 x v2).
Cross product between v1 and v2 (v1 x v2), the used indices are the same as
the input vectors.
Notes
-----
...
...
@@ -194,25 +208,31 @@ def cross_product(v1, v2, vres=None, size_4=True):
"""
if
vres
is
None
:
vres
=
np
.
zeros
(
3
+
size_4
)
vres
[
size_4
:]
=
[
v1
[
2
]
*
v2
[
3
]
-
v1
[
3
]
*
v2
[
2
],
v1
[
3
]
*
v2
[
1
]
-
v1
[
1
]
*
v2
[
3
],
v1
[
1
]
*
v2
[
2
]
-
v1
[
2
]
*
v2
[
1
]
]
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].
return
vres
def
matrix_vector_product
(
matrix
,
vector
):
"""Compute the product of a matrix and a vector with first index
is
1.
"""Compute the product of a matrix and a vector with first index
are
1.
Parameters
----------
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].
Returns
-------
...
...
@@ -231,11 +251,11 @@ def matrix_vector_product(matrix, vector):
Notes
-----
Globally the functions simply calls numpy.dot(matrix[1:, 1:], vector[1:
, 1:
])
Globally the functions simply calls numpy.dot(matrix[1:
4
, 1:
4
], vector[1:
4
])
"""
vector_res
=
np
.
zeros
(
4
)
vector_res
[
1
:]
=
np
.
dot
(
matrix
[
1
:,
1
:],
vector
[
1
:])[:]
vector_res
[
1
:
4
]
=
np
.
dot
(
matrix
[
1
:
4
,
1
:
4
],
vector
[
1
:
4
])[:]
return
vector_res
...
...
@@ -246,8 +266,10 @@ def matrix_matrix_product(matrix_1, matrix_2):
----------
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
-------
...
...
@@ -266,33 +288,39 @@ def matrix_matrix_product(matrix_1, matrix_2):
Notes
-----
Globally the functions simply calls numpy.dot(matrix_1[1:, 1:], matrix_2[1:, 1:])
Globally the functions simply calls numpy.dot(matrix_1[1:
4
, 1:
4
], matrix_2[1:
4
, 1:
4
])
"""
matrix_res
=
np
.
zeros
((
4
,
4
))
matrix_res
[
1
:,
1
:]
=
np
.
dot
(
matrix_1
[
1
:,
1
:],
matrix_2
[
1
:,
1
:])[:,
:]
matrix_res
[
1
:,
1
:]
=
np
.
dot
(
matrix_1
[
1
:
4
,
1
:
4
],
matrix_2
[
1
:
4
,
1
:
4
])[:,
:]
return
matrix_res
def
matrix_product
(
M1
,
M2
,
M
res
=
None
,
size_4
=
True
):
def
matrix_product
(
M1
,
M2
,
res
=
None
,
index_1
=
True
):
"""Compute the product of 2 matrices.
The function handle both index 0 and index 1 array according to the parameter
'size_4'.
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].
Parameters
----------
M1 : numpy.ndarray
Matrix to be multiplied by M2.
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.
Mres : numpy.ndarray, optional
Matrice to store the result if not None.
It must have the right dimensions.
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
size_4 : bool, optional
True if vectors have a size of 4 and matrices are 4x4.
index_1 : bool, optional
If False, the components [0:3] are considered for the computation (instead
of [1:4]).
default: True.
Raises
...
...
@@ -302,9 +330,9 @@ def matrix_product(M1, M2, Mres=None, size_4=True):
Returns
-------
M
res : numpy.ndarray
res : numpy.ndarray
Matrix product between M1 and M2 (M1 . M2). The array is allocated if
it was not provided.
it was not provided
, the used indices are the same as the input arrays
.
See Also
--------
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment