mbs_algebra.py 11.6 KB
Newer Older
Louis Beauloye's avatar
Louis Beauloye committed
1
2
3
4
5
6
# -*- coding: utf-8 -*-
"""
Define algebra functions for package MBsysPy.

Summary
-------
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's avatar
Louis Beauloye committed
9
10
11
12
"""
# Author : Robotran Team
# (c) Universite catholique de Louvain, 2020
import numpy as np
13
from math import cos, sin
Louis Beauloye's avatar
Louis Beauloye committed
14
15


16
17
18
19
20
def norm(v, size_4=True):
    """Compute the norm of a vector, starting with index 1.

    Parameters
    ----------
Louis Beauloye's avatar
Louis Beauloye committed
21
    v : numpy.ndarray or list
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's avatar
Louis Beauloye committed
32
        If v is not a numpy.ndarray or a list.
33
34
35
36
37
38
39

    Returns
    -------
    float
        Norm of vector v starting at index 1.

    """
40
    v, mat = __get_index_0_matrix__(v, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
41
    if mat:
42
43
        raise ValueError('Vector must be passed as argument for norm function.')
    return np.linalg.norm(v)
44
45


Louis Beauloye's avatar
Louis Beauloye committed
46
47
48
def normalize(v, vres=None, size_4=True):
    """Normalize vector and return a copy, starting with index 1.

49
50
    Parameters
    ----------
Louis Beauloye's avatar
Louis Beauloye committed
51
    v : numpy.ndarray or list
52
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
53
    vres : list or numpy.ndarray, optional
Louis Beauloye's avatar
Louis Beauloye committed
54
55
        Vector containing the result if not None.
        default: None
56
    size_4 : bool, optional
Louis Beauloye's avatar
Louis Beauloye committed
57
        True if v has a size of 4.
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's avatar
Louis Beauloye committed
67
    v_norm : numpy.ndarray
Louis Beauloye's avatar
Louis Beauloye committed
68
        Copy of normalized vector v starting at index 1.
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's avatar
Louis Beauloye committed
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
84
85


Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
94
    v1 : list or numpy.ndarray
95
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
96
    v2 : list or numpy.ndarray
97
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
98
    size_4 : bool, optional
Louis Beauloye's avatar
Louis Beauloye committed
99
100
        True if vectors have a size of 4.
        default: True.
Louis Beauloye's avatar
Louis Beauloye committed
101
102
103
104

    Raises
    ------
    ValueError
105
106
        If v1 or v2 is not a 1-dim array of size 4 if size_4 is True.
    TypeError
Louis Beauloye's avatar
Louis Beauloye committed
107
        If v1 or v2 is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
108
109
110
111

    Returns
    -------
    float
112
        Scalar product between v1 and v2.
Louis Beauloye's avatar
Louis Beauloye committed
113
114

    """
115
116
    v1, mat1 = __get_index_0_matrix__(v1, size_4)
    v2, mat2 = __get_index_0_matrix__(v2, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
120
121


122
def vector_sum(v1, v2, vres=None, size_4=True):
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
130
    v1 : list or numpy.ndarray
131
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
132
    v2 : list or numpy.ndarray
133
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
134
    vres : list or numpy.ndarray, optional
135
136
        Vector containing the result if not None.
        default: None
Louis Beauloye's avatar
Louis Beauloye committed
137
    size_4 : bool, optional
138
139
        True if vectors have a size of 4.
        default: True.
Louis Beauloye's avatar
Louis Beauloye committed
140
141
142
143

    Raises
    ------
    ValueError
144
145
        If v1 or v2 is not a 1-dim array of size 4 if size_4 is True.
    TypeError
Louis Beauloye's avatar
Louis Beauloye committed
146
        If v1 or v2 is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
147
148
149

    Returns
    -------
Louis Beauloye's avatar
Louis Beauloye committed
150
    v_sum : numpy.ndarray
151
        Sum of v1 and v2.
Louis Beauloye's avatar
Louis Beauloye committed
152
153

    """
154
155
    v1, mat1 = __get_index_0_matrix__(v1, size_4)
    v2, mat2 = __get_index_0_matrix__(v2, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
162
        else:
Louis Beauloye's avatar
Louis Beauloye committed
163
164
            if (vres.shape)[0] == 1:
                vres[0, 1:] = v_sum
Louis Beauloye's avatar
Louis Beauloye committed
165
            else:
Louis Beauloye's avatar
Louis Beauloye committed
166
                vres[1:, 0] = v_sum
Louis Beauloye's avatar
Louis Beauloye committed
167
    else:
168
        v_sum = np.append(v_sum.shape[0], v_sum)
Louis Beauloye's avatar
Louis Beauloye committed
169
        return v_sum
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
180
    v1 : list or numpy.ndarray
181
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
182
    v2 : list or numpy.ndarray
183
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
184
    vres : list or numpy.ndarray, optional
185
186
        Vector containing the result if not None.
        default: None
Louis Beauloye's avatar
Louis Beauloye committed
187
    size_4 : bool, optional
188
189
        True if vectors have a size of 4.
        default: True.
Louis Beauloye's avatar
Louis Beauloye committed
190
191
192
193

    Raises
    ------
    ValueError
194
195
        If v1 or v2 is not a 1-dim array of size 4 if size_4 is True.
    TypeError
Louis Beauloye's avatar
Louis Beauloye committed
196
        If v1 or v2 is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
197
198
199

    Returns
    -------
Louis Beauloye's avatar
Louis Beauloye committed
200
    v_diff : numpy.ndarray
201
        Substraction of v1 and v2 (v1 - v2).
Louis Beauloye's avatar
Louis Beauloye committed
202
203

    """
204
205
    v1, mat1 = __get_index_0_matrix__(v1, size_4)
    v2, mat2 = __get_index_0_matrix__(v2, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
212
        else:
Louis Beauloye's avatar
Louis Beauloye committed
213
214
            if (vres.shape)[0] == 1:
                vres[0, 1:] = v_diff
Louis Beauloye's avatar
Louis Beauloye committed
215
            else:
Louis Beauloye's avatar
Louis Beauloye committed
216
                vres[1:, 0] = v_diff
Louis Beauloye's avatar
Louis Beauloye committed
217
    else:
218
        v_diff = np.append(v_diff.shape[0], v_diff)
Louis Beauloye's avatar
Louis Beauloye committed
219
        return v_diff
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
230
    v1 : list or numpy.ndarray
231
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
232
    v2 : list or numpy.ndarray
233
        Vector with first element unused.
Louis Beauloye's avatar
Louis Beauloye committed
234
    vres : list or numpy.ndarray, optional
235
236
        Vector containing the result if not None.
        default: None
Louis Beauloye's avatar
Louis Beauloye committed
237
    size_4 : bool, optional
238
239
        True if vectors have a size of 4.
        default: True.
Louis Beauloye's avatar
Louis Beauloye committed
240
241
242
243

    Raises
    ------
    ValueError
244
245
        If v1 or v2 is not a 1-dim array of size 4 if size_4 is True.
    TypeError
Louis Beauloye's avatar
Louis Beauloye committed
246
        If v1 or v2 is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
247
248
249

    Returns
    -------
Louis Beauloye's avatar
Louis Beauloye committed
250
    v_cross : numpy.ndarray
251
        Cross product between v1 and v2 (v1 x v2).
Louis Beauloye's avatar
Louis Beauloye committed
252
253

    """
254
255
    v1, mat1 = __get_index_0_matrix__(v1, size_4)
    v2, mat2 = __get_index_0_matrix__(v2, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
262
        else:
Louis Beauloye's avatar
Louis Beauloye committed
263
264
            if (vres.shape)[0] == 1:
                vres[0, 1:] = v_cross
Louis Beauloye's avatar
Louis Beauloye committed
265
            else:
Louis Beauloye's avatar
Louis Beauloye committed
266
                vres[1:, 0] = v_cross
Louis Beauloye's avatar
Louis Beauloye committed
267
    else:
268
        v_cross = np.append(v_cross.shape[0], v_cross)
Louis Beauloye's avatar
Louis Beauloye committed
269
        return v_cross
Louis Beauloye's avatar
Louis Beauloye committed
270
271


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's avatar
Louis Beauloye committed
274

275
276
    The second parameter can be a vector.
    Matrices and vector have unused index 0.
Louis Beauloye's avatar
Louis Beauloye committed
277
278
279
280


    Parameters
    ----------
Louis Beauloye's avatar
Louis Beauloye committed
281
    M1 : list or numpy.ndarray
282
        Matrix with first line and row unused, will be multiplied by M2.
Louis Beauloye's avatar
Louis Beauloye committed
283
    M2 : list or numpy.ndarray
284
285
        Matrix with first line and row unused.
        It could also be a vector with first element unused
Louis Beauloye's avatar
Louis Beauloye committed
286
    Mres : list or numpy.ndarray, optional
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's avatar
Louis Beauloye committed
291
    size_4 : bool, optional
292
293
        True if vectors have a size of 4 and matrices are 4x4.
        default: True.
Louis Beauloye's avatar
Louis Beauloye committed
294
295
296
297

    Raises
    ------
    ValueError
298
299
        If M1 or M2 is not an array of size 4 if size_4 is True.
    TypeError
Louis Beauloye's avatar
Louis Beauloye committed
300
        If M1 or M2 is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
301
302
303

    Returns
    -------
Louis Beauloye's avatar
Louis Beauloye committed
304
    M_prod : numpy.ndarray
Louis Beauloye's avatar
Louis Beauloye committed
305
        Matrix product between M1 and M2 (M . M2) if Mres is None.
Louis Beauloye's avatar
Louis Beauloye committed
306
307

    """
308
309
    M1, mat1 = __get_index_0_matrix__(M1, size_4)
    M2, mat2 = __get_index_0_matrix__(M2, size_4)
Louis Beauloye's avatar
Louis Beauloye committed
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
317
            else:
Louis Beauloye's avatar
Louis Beauloye committed
318
319
                if (Mres.shape)[0] == 1:
                    Mres[0, 1:] = M_prod
Louis Beauloye's avatar
Louis Beauloye committed
320
                else:
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
326
        else:
327
            M_prod = np.append(M_prod.shape[0], M_prod)
Louis Beauloye's avatar
Louis Beauloye committed
328
329
330
331

        return M_prod


332
def __get_index_0_matrix__(M, size_4):
Louis Beauloye's avatar
Louis Beauloye committed
333
334
335
336
    """Remove first index and check if right dimensions.

    Parameters
    ----------
Louis Beauloye's avatar
Louis Beauloye committed
337
    M : list or numpy.ndarray
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
348
        If M is not a numpy.ndarray or a list.
Louis Beauloye's avatar
Louis Beauloye committed
349
350
351

    Returns
    -------
Louis Beauloye's avatar
Louis Beauloye committed
352
    M1 : numpy.ndarray
Louis Beauloye's avatar
Louis Beauloye committed
353
354
355
356
357
358
        M without first index.
    mat : bool
        True if M1 is a matrix.

    """
    mat = True
Louis Beauloye's avatar
fix bug    
Louis Beauloye committed
359
    if isinstance(M, (list, np.ndarray)):
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
369
            else:
Louis Beauloye's avatar
Louis Beauloye committed
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's avatar
Louis Beauloye committed
377
    else:
Louis Beauloye's avatar
Louis Beauloye committed
378
        raise TypeError('Matrices must be numpy.ndarray or list')
Louis Beauloye's avatar
Louis Beauloye committed
379
380
381


def rotation_matrix(rot_type, angle, Rres=None):
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's avatar
Louis Beauloye committed
386
387
388
389

    Parameters
    ----------
    rot_type : int
390
        Integer for axis selection
Louis Beauloye's avatar
Louis Beauloye committed
391
392
393
394
        1: rotation along x-axis.
        2: rotation along y-axis.
        3: rotation along z-axis.
    angle : float
395
        Rotation angle expressed in radian.
396
397
    Mres : numpy.ndarray, optional
        Matrix containing the result if not None.
398
        default: None
Louis Beauloye's avatar
Louis Beauloye committed
399
400
401
    Raises
    ------
    ValueError
402
        If rot_type different from 1, 2 or 3.
Louis Beauloye's avatar
Louis Beauloye committed
403
404
405

    Returns
    -------
406
    Rres : numpy.ndarray
407
        Rotation matrix.
Louis Beauloye's avatar
Louis Beauloye committed
408
409

    """
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's avatar
Louis Beauloye committed
421
422

    if rot_type == 1:
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's avatar
Louis Beauloye committed
428
    elif rot_type == 2:
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's avatar
Louis Beauloye committed
434
    elif rot_type == 3:
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