Rigid Body Dynamics Library
rbdl_mathutils.cc
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #include <cmath>
9 #include <limits>
10 
11 #include <iostream>
12 #include <assert.h>
13 
14 #include <rbdl/rbdl_mathutils.h>
15 #include <rbdl/Model.h>
16 
17 #include "rbdl/Logging.h"
18 
19 namespace RigidBodyDynamics {
20 namespace Math {
21 
22 RBDL_DLLAPI Vector3d Vector3dZero (0., 0., 0.);
23 RBDL_DLLAPI Matrix3d Matrix3dIdentity (
24  1., 0., 0.,
25  0., 1., 0.,
26  0., 0., 1
27  );
28 RBDL_DLLAPI Matrix3d Matrix3dZero (
29  0., 0., 0.,
30  0., 0., 0.,
31  0., 0., 0.
32  );
33 
34 RBDL_DLLAPI SpatialVector SpatialVectorZero ( 0., 0., 0., 0., 0., 0.);
35 
36 RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) {
37  x = VectorNd::Zero(x.size());
38 
39  // We can only solve quadratic systems
40  assert (A.rows() == A.cols());
41 
42  unsigned int n = A.rows();
43  unsigned int pi;
44 
45  // the pivots
46  size_t *pivot = new size_t[n];
47 
48  // temporary result vector which contains the pivoted result
49  VectorNd px(x);
50 
51  unsigned int i,j,k;
52 
53  for (i = 0; i < n; i++)
54  pivot[i] = i;
55 
56  for (j = 0; j < n; j++) {
57  pi = j;
58  double pv = fabs (A(j,pivot[j]));
59 
60  // LOG << "j = " << j << " pv = " << pv << std::endl;
61  // find the pivot
62  for (k = j; k < n; k++) {
63  double pt = fabs (A(j,pivot[k]));
64  if (pt > pv) {
65  pv = pt;
66  pi = k;
67  unsigned int p_swap = pivot[j];
68  pivot[j] = pivot[pi];
69  pivot[pi] = p_swap;
70  // LOG << "swap " << j << " with " << pi << std::endl;
71  // LOG << "j = " << j << " pv = " << pv << std::endl;
72  }
73  }
74 
75  for (i = j + 1; i < n; i++) {
76  if (fabs(A(j,pivot[j])) <= std::numeric_limits<double>::epsilon()) {
77  std::cerr << "Error: pivoting failed for matrix A = " << std::endl;
78  std::cerr << "A = " << std::endl << A << std::endl;
79  std::cerr << "b = " << b << std::endl;
80  }
81  // assert (fabs(A(j,pivot[j])) > std::numeric_limits<double>::epsilon());
82  double d = A(i,pivot[j])/A(j,pivot[j]);
83 
84  b[i] -= b[j] * d;
85 
86  for (k = j; k < n; k++) {
87  A(i,pivot[k]) -= A(j,pivot[k]) * d;
88  }
89  }
90  }
91 
92  // warning: i is an unsigned int, therefore a for loop of the
93  // form "for (i = n - 1; i >= 0; i--)" might end up in getting an invalid
94  // value for i!
95  i = n;
96  do {
97  i--;
98 
99  for (j = i + 1; j < n; j++) {
100  px[i] += A(i,pivot[j]) * px[j];
101  }
102  px[i] = (b[i] - px[i]) / A(i,pivot[i]);
103 
104  } while (i > 0);
105 
106  // Unswapping
107  for (i = 0; i < n; i++) {
108  x[pivot[i]] = px[i];
109  }
110 
111  /*
112  LOG << "A = " << std::endl << A << std::endl;
113  LOG << "b = " << b << std::endl;
114  LOG << "x = " << x << std::endl;
115  LOG << "pivot = " << pivot[0] << " " << pivot[1] << " " << pivot[2] << std::endl;
116  std::cout << LogOutput.str() << std::endl;
117  */
118 
119  delete[] pivot;
120 
121  return true;
122 }
123 
124 RBDL_DLLAPI void SpatialMatrixSetSubmatrix(
125  SpatialMatrix &dest,
126  unsigned int row,
127  unsigned int col,
128  const Matrix3d &matrix) {
129  assert (row < 2 && col < 2);
130 
131  dest(row*3,col*3) = matrix(0,0);
132  dest(row*3,col*3 + 1) = matrix(0,1);
133  dest(row*3,col*3 + 2) = matrix(0,2);
134 
135  dest(row*3 + 1,col*3) = matrix(1,0);
136  dest(row*3 + 1,col*3 + 1) = matrix(1,1);
137  dest(row*3 + 1,col*3 + 2) = matrix(1,2);
138 
139  dest(row*3 + 2,col*3) = matrix(2,0);
140  dest(row*3 + 2,col*3 + 1) = matrix(2,1);
141  dest(row*3 + 2,col*3 + 2) = matrix(2,2);
142 }
143 
144 RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (
145  const SpatialMatrix &matrix_a,
146  const SpatialMatrix &matrix_b,
147  double epsilon) {
148  assert (epsilon >= 0.);
149  unsigned int i, j;
150 
151  for (i = 0; i < 6; i++) {
152  for (j = 0; j < 6; j++) {
153  if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) {
154  std::cerr << "Expected:"
155  << std::endl << matrix_a << std::endl
156  << "but was" << std::endl
157  << matrix_b << std::endl;
158  return false;
159  }
160  }
161  }
162 
163  return true;
164 }
165 
166 RBDL_DLLAPI bool SpatialVectorCompareEpsilon (
167  const SpatialVector &vector_a,
168  const SpatialVector &vector_b,
169  double epsilon) {
170  assert (epsilon >= 0.);
171  unsigned int i;
172 
173  for (i = 0; i < 6; i++) {
174  if (fabs(vector_a[i] - vector_b[i]) >= epsilon) {
175  std::cerr << "Expected:"
176  << std::endl << vector_a << std::endl
177  << "but was" << std::endl
178  << vector_b << std::endl;
179  return false;
180  }
181  }
182 
183  return true;
184 }
185 
186 RBDL_DLLAPI Matrix3d parallel_axis (
187  const Matrix3d &inertia,
188  double mass,
189  const Vector3d &com) {
190  Matrix3d com_cross = VectorCrossMatrix (com);
191 
192  return inertia + mass * com_cross * com_cross.transpose();
193 }
194 
195 RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &r) {
196  return SpatialMatrix (
197  1., 0., 0., 0., 0., 0.,
198  0., 1., 0., 0., 0., 0.,
199  0., 0., 1., 0., 0., 0.,
200  0., r[2], -r[1], 1., 0., 0.,
201  -r[2], 0., r[0], 0., 1., 0.,
202  r[1], -r[0], 0., 0., 0., 1.
203  );
204 }
205 
206 RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) {
207  double s, c;
208  s = sin (xrot);
209  c = cos (xrot);
210 
211  return SpatialMatrix(
212  1., 0., 0., 0., 0., 0.,
213  0., c, s, 0., 0., 0.,
214  0., -s, c, 0., 0., 0.,
215  0., 0., 0., 1., 0., 0.,
216  0., 0., 0., 0., c, s,
217  0., 0., 0., 0., -s, c
218  );
219 }
220 
221 RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) {
222  double s, c;
223  s = sin (yrot);
224  c = cos (yrot);
225 
226  return SpatialMatrix(
227  c, 0., -s, 0., 0., 0.,
228  0., 1., 0., 0., 0., 0.,
229  s, 0., c, 0., 0., 0.,
230  0., 0., 0., c, 0., -s,
231  0., 0., 0., 0., 1., 0.,
232  0., 0., 0., s, 0., c
233  );
234 }
235 
236 RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot) {
237  double s, c;
238  s = sin (zrot);
239  c = cos (zrot);
240 
241  return SpatialMatrix(
242  c, s, 0., 0., 0., 0.,
243  -s, c, 0., 0., 0., 0.,
244  0., 0., 1., 0., 0., 0.,
245  0., 0., 0., c, s, 0.,
246  0., 0., 0., -s, c, 0.,
247  0., 0., 0., 0., 0., 1.
248  );
249 }
250 
252  const Vector3d &displacement,
253  const Vector3d &zyx_euler) {
254  return Xrotz_mat(zyx_euler[0]) * Xroty_mat(zyx_euler[1]) * Xrotx_mat(zyx_euler[2]) * Xtrans_mat(displacement);
255 }
256 
257 RBDL_DLLAPI void SparseFactorizeLTL (Model &model, Math::MatrixNd &H) {
258  for (unsigned int i = 0; i < model.qdot_size; i++) {
259  for (unsigned int j = i + 1; j < model.qdot_size; j++) {
260  H(i,j) = 0.;
261  }
262  }
263 
264  for (unsigned int k = model.qdot_size; k > 0; k--) {
265  H(k - 1,k - 1) = sqrt (H(k - 1,k - 1));
266  unsigned int i = model.lambda_q[k];
267  while (i != 0) {
268  H(k - 1,i - 1) = H(k - 1,i - 1) / H(k - 1,k - 1);
269  i = model.lambda_q[i];
270  }
271 
272  i = model.lambda_q[k];
273  while (i != 0) {
274  unsigned int j = i;
275  while (j != 0) {
276  H(i - 1,j - 1) = H(i - 1,j - 1) - H(k - 1,i - 1) * H(k - 1, j - 1);
277  j = model.lambda_q[j];
278  }
279  i = model.lambda_q[i];
280  }
281  }
282 }
283 
284 RBDL_DLLAPI void SparseMultiplyHx (Model &model, Math::MatrixNd &L) {
285  assert (0 && !"Not yet implemented!");
286 }
287 
288 RBDL_DLLAPI void SparseMultiplyLx (Model &model, Math::MatrixNd &L) {
289  assert (0 && !"Not yet implemented!");
290 }
291 
292 RBDL_DLLAPI void SparseMultiplyLTx (Model &model, Math::MatrixNd &L) {
293  assert (0 && !"Not yet implemented!");
294 }
295 
296 RBDL_DLLAPI void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) {
297  for (unsigned int i = 1; i <= model.qdot_size; i++) {
298  unsigned int j = model.lambda_q[i];
299  while (j != 0) {
300  x[i - 1] = x[i - 1] - L(i - 1,j - 1) * x[j - 1];
301  j = model.lambda_q[j];
302  }
303  x[i - 1] = x[i - 1] / L(i - 1,i - 1);
304  }
305 }
306 
307 RBDL_DLLAPI void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) {
308  for (int i = model.qdot_size; i > 0; i--) {
309  x[i - 1] = x[i - 1] / L(i - 1,i - 1);
310  unsigned int j = model.lambda_q[i];
311  while (j != 0) {
312  x[j - 1] = x[j - 1] - L(i - 1,j - 1) * x[i - 1];
313  j = model.lambda_q[j];
314  }
315  }
316 }
317 
318 } /* Math */
319 } /* RigidBodyDynamics */
RBDL_DLLAPI Vector3d Vector3dZero
RBDL_DLLAPI SpatialMatrix Xrotx_mat(const double &xrot)
Creates a rotational transformation around the X-axis.
RBDL_DLLAPI Matrix3d Matrix3dIdentity
RBDL_DLLAPI SpatialMatrix Xrotz_mat(const double &zrot)
Creates a rotational transformation around the Z-axis.
RBDL_DLLAPI void SparseMultiplyLTx(Model &model, Math::MatrixNd &L)
RBDL_DLLAPI SpatialMatrix Xtrans_mat(const Vector3d &r)
Creates a transformation of a linear displacement.
RBDL_DLLAPI SpatialMatrix Xroty_mat(const double &yrot)
Creates a rotational transformation around the Y-axis.
RBDL_DLLAPI bool SpatialMatrixCompareEpsilon(const SpatialMatrix &matrix_a, const SpatialMatrix &matrix_b, double epsilon)
RBDL_DLLAPI void SparseFactorizeLTL(Model &model, Math::MatrixNd &H)
Contains all information about the rigid body model.
Definition: Model.h:121
RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler(const Vector3d &displacement, const Vector3d &zyx_euler)
Creates a spatial transformation for given parameters.
RBDL_DLLAPI void SparseMultiplyLx(Model &model, Math::MatrixNd &L)
Matrix3d VectorCrossMatrix(const Vector3d &vector)
RBDL_DLLAPI Matrix3d parallel_axis(const Matrix3d &inertia, double mass, const Vector3d &com)
Translates the inertia matrix to a new center.
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
std::vector< unsigned int > lambda_q
The index of the parent degree of freedom that is directly influencing the current one...
Definition: Model.h:130
RBDL_DLLAPI bool SpatialVectorCompareEpsilon(const SpatialVector &vector_a, const SpatialVector &vector_b, double epsilon)
unsigned int qdot_size
The size of the.
Definition: Model.h:156
SpatialMatrix_t SpatialMatrix
Definition: rbdl_math.h:60
RBDL_DLLAPI void SparseMultiplyHx(Model &model, Math::MatrixNd &L)
RBDL_DLLAPI SpatialVector SpatialVectorZero(0., 0., 0., 0., 0., 0.)
RBDL_DLLAPI void SparseSolveLx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
RBDL_DLLAPI Matrix3d Matrix3dZero
RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix)
RBDL_DLLAPI bool LinSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd &x)
Solves a linear system using gaussian elimination with pivoting.
RBDL_DLLAPI void SparseSolveLTx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)