matrix_util.py
Go to the documentation of this file.
1 """
2 Tiny matrix functions, taken from Oscar source code.
3 """
4 
5 from math import atan2
6 from random import random
7 
8 from numpy import array, cos, inner, matrix, pi, sin, sqrt
9 from numpy.linalg import norm
10 
11 
12 # Convert matrix to tuple
14  tmp = M.tolist()
15  res = []
16  for i in tmp:
17  res.append(tuple(i))
18  return tuple(res)
19 
20 
22  if len(M.shape) == 1:
23  return tuple(M.tolist())
24  elif M.shape[0] == 1:
25  return tuple(M.tolist()[0])
26  else:
27  return tuple(M.transpose().tolist()[0])
28 
29 
30 # Convert from Roll, Pitch, Yaw to transformation Matrix
31 def rpy2tr(r, p, y):
32  mat = matrix(rotate("z", y)) * matrix(rotate("y", p)) * matrix(rotate("x", r))
33  return matrixToTuple(mat)
34 
35 
36 # Get the distance btw the position components of 2 transf matrices
37 def distVector(M2, M1):
38  px = M1[0][3] - M2[0][3]
39  py = M1[1][3] - M2[1][3]
40  pz = M1[2][3] - M2[2][3]
41  return [px, py, pz]
42 
43 
44 # Obtain an orthonormal matrix SO3 using the given vector as its first column
45 # (it computes Gram Schmidt to obtain an orthonormal basis using the
46 # first vector and 2 other 'random' vectors)
47 
48 
50  v2 = matrix([random(), random(), random()])
51  v3 = matrix([random(), random(), random()])
52 
53  v1 = matrix(v1)
54  e1 = v1 / norm(v1)
55 
56  u2 = v2 - inner(v2, e1) * e1
57  e2 = u2 / norm(u2)
58 
59  u3 = v3 - inner(v3, e1) * e1 - inner(v3, e2) * e2
60  e3 = u3 / norm(u3)
61 
62  e1 = e1.tolist()
63  e2 = e2.tolist()
64  e3 = e3.tolist()
65  M = (
66  (e1[0][0], e2[0][0], e3[0][0]),
67  (e1[0][1], e2[0][1], e3[0][1]),
68  (e1[0][2], e2[0][2], e3[0][2]),
69  )
70  return M
71 
72 
73 # Convert from Transformation Matrix to Roll,Pitch,Yaw
74 def tr2rpy(M):
75  m = sqrt(M[2][1] ** 2 + M[2][2] ** 2)
76  p = atan2(-M[2][0], m)
77 
78  if abs(p - pi / 2) < 0.001:
79  r = 0
80  y = atan2(M[0][1], M[1][1])
81  elif abs(p + pi / 2) < 0.001:
82  r = 0
83  y = -atan2(M[0][1], M[1][1])
84  else:
85  r = atan2(M[1][0], M[0][0])
86  y = atan2(M[2][1], M[2][2])
87 
88  return [float(r), float(p), float(y)]
89 
90 
91 def matrixToRPY(M):
92  """
93  Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
94  """
95  rot = tr2rpy(M)
96  return [M[0][3], M[1][3], M[2][3], rot[2], rot[1], rot[0]]
97 
98 
99 def RPYToMatrix(pr):
100  """
101  Convert a 6x1 rpy pose vector to a 4x4 homogeneous matrix.
102  """
103  M = array(rpy2tr(pr[3], pr[4], pr[5]))
104  M[0:3, 3] = pr[0:3]
105  return M
106 
107 
108 # Transformation Matrix corresponding to a rotation about x,y or z
109 def rotate(axis, ang):
110  """eg. T=rot('x',pi/4): rotate pi/4 rad about x axis"""
111  ca = cos(ang)
112  sa = sin(ang)
113  if axis == "x":
114  mat = ((1, 0, 0, 0), (0, ca, -sa, 0), (0, sa, ca, 0), (0, 0, 0, 1))
115  elif axis == "y":
116  mat = ((ca, 0, sa, 0), (0, 1, 0, 0), (-sa, 0, ca, 0), (0, 0, 0, 1))
117  elif axis == "z":
118  mat = ((ca, -sa, 0, 0), (sa, ca, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
119  else:
120  print("Axis should be: x,y or z only")
121  return mat
122 
123 
125  [qx, qy, qz, qw] = q
126  R = [
127  [
128  1 - 2 * qy**2 - 2 * qz**2,
129  2 * qx * qy - 2 * qz * qw,
130  2 * qx * qz + 2 * qy * qw,
131  ],
132  [
133  2 * qx * qy + 2 * qz * qw,
134  1 - 2 * qx**2 - 2 * qz**2,
135  2 * qy * qz - 2 * qx * qw,
136  ],
137  [
138  2 * qx * qz - 2 * qy * qw,
139  2 * qy * qz + 2 * qx * qw,
140  1 - 2 * qx**2 - 2 * qy**2,
141  ],
142  ]
143  return R
type random()


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26