Rigid Body Dynamics Library
rbdl_mathutils.h
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 #ifndef RBDL_MATHUTILS_H
9 #define RBDL_MATHUTILS_H
10 
11 #include <assert.h>
12 #include <cmath>
13 
14 #include "rbdl/rbdl_math.h"
15 
16 namespace RigidBodyDynamics {
17 struct Model;
18 
19 namespace Math {
20 
27 enum RBDL_DLLAPI LinearSolver {
34 };
35 
36 extern RBDL_DLLAPI Vector3d Vector3dZero;
37 extern RBDL_DLLAPI Matrix3d Matrix3dIdentity;
38 extern RBDL_DLLAPI Matrix3d Matrix3dZero;
39 
40 RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) {
41  // TODO: use memory mapping operators for Eigen
42  VectorNd result (n);
43 
44  for (unsigned int i = 0; i < n; i++) {
45  result[i] = ptr[i];
46  }
47 
48  return result;
49 }
50 
51 RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major = true) {
52  MatrixNd result (rows, cols);
53 
54  if (row_major) {
55  for (unsigned int i = 0; i < rows; i++) {
56  for (unsigned int j = 0; j < cols; j++) {
57  result(i,j) = ptr[i * cols + j];
58  }
59  }
60  } else {
61  for (unsigned int i = 0; i < rows; i++) {
62  for (unsigned int j = 0; j < cols; j++) {
63  result(i,j) = ptr[i + j * rows];
64  }
65  }
66  }
67 
68  return result;
69 }
70 
72 RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
73 
74 // \todo write test
75 RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix);
76 
77 RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a,
78  const SpatialMatrix &matrix_b, double epsilon);
79 RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a,
80  const SpatialVector &vector_b, double epsilon);
81 
83 RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com);
84 
95 RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement);
96 
104 RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot);
105 
113 RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot);
114 
122 RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot);
123 
133 RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler);
134 
135 RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) {
136  double s, c;
137  s = sin (xrot);
138  c = cos (xrot);
139  return Matrix3d (
140  1., 0., 0.,
141  0., c, s,
142  0., -s, c
143  );
144 }
145 
146 RBDL_DLLAPI inline Matrix3d roty (const double &yrot) {
147  double s, c;
148  s = sin (yrot);
149  c = cos (yrot);
150  return Matrix3d (
151  c, 0., -s,
152  0., 1., 0.,
153  s, 0., c
154  );
155 }
156 
157 RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) {
158  double s, c;
159  s = sin (zrot);
160  c = cos (zrot);
161  return Matrix3d (
162  c, s, 0.,
163  -s, c, 0.,
164  0., 0., 1.
165  );
166 }
167 
168 RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) {
169  double s, c;
170  s = sin (x);
171  c = cos (x);
172  return Matrix3d (
173  0., 0., 0.,
174  0., -s * xdot, c * xdot,
175  0., -c * xdot,-s * xdot
176  );
177 }
178 
179 RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) {
180  double s, c;
181  s = sin (y);
182  c = cos (y);
183  return Matrix3d (
184  -s * ydot, 0., - c * ydot,
185  0., 0., 0.,
186  c * ydot, 0., - s * ydot
187  );
188 }
189 
190 RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) {
191  double s, c;
192  s = sin (z);
193  c = cos (z);
194  return Matrix3d (
195  -s * zdot, c * zdot, 0.,
196  -c * zdot, -s * zdot, 0.,
197  0., 0., 0.
198  );
199 }
200 
201 RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) {
202  double sy = sin(zyx_angles[1]);
203  double cy = cos(zyx_angles[1]);
204  double sx = sin(zyx_angles[2]);
205  double cx = cos(zyx_angles[2]);
206 
207  return Vector3d (
208  zyx_angle_rates[2] - sy * zyx_angle_rates[0],
209  cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0],
210  -sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0]
211  );
212 }
213 
214 RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) {
215  Matrix3d RzT = rotz(zyx_angles[0]).transpose();
216  Matrix3d RyT = roty(zyx_angles[1]).transpose();
217 
218  return Vector3d (
219  Vector3d (0., 0., zyx_rates[0])
220  + RzT * Vector3d (0., zyx_rates[1], 0.)
221  + RzT * RyT * Vector3d (zyx_rates[2], 0., 0.)
222  );
223 }
224 
225 RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) {
226  double sy = sin(zyx_angles[1]);
227  double cy = cos(zyx_angles[1]);
228  double sx = sin(zyx_angles[2]);
229  double cx = cos(zyx_angles[2]);
230  double xdot = zyx_angle_rates[2];
231  double ydot = zyx_angle_rates[1];
232  double zdot = zyx_angle_rates[0];
233  double xddot = zyx_angle_rates_dot[2];
234  double yddot = zyx_angle_rates_dot[1];
235  double zddot = zyx_angle_rates_dot[0];
236 
237  return Vector3d (
238  xddot - (cy * ydot * zdot + sy * zddot),
239  -sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot),
240  -cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot)
241  );
242 }
243 
244 RBDL_DLLAPI
245 void SparseFactorizeLTL (Model &model, Math::MatrixNd &H);
246 
247 RBDL_DLLAPI
248 void SparseMultiplyHx (Model &model, Math::MatrixNd &L);
249 
250 RBDL_DLLAPI
251 void SparseMultiplyLx (Model &model, Math::MatrixNd &L);
252 RBDL_DLLAPI
253 void SparseMultiplyLTx (Model &model, Math::MatrixNd &L);
254 
255 RBDL_DLLAPI
256 void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x);
257 RBDL_DLLAPI
258 void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x);
259 
260 } /* Math */
261 
262 } /* RigidBodyDynamics */
263 
264 /* RBDL_MATHUTILS_H */
265 #endif
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 Matrix3d rotx(const double &xrot)
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.
LinearSolverPartialPivLU
RBDL_DLLAPI void SparseMultiplyLx(Model &model, Math::MatrixNd &L)
LinearSolverHouseholderQR
RBDL_DLLAPI MatrixNd MatrixFromPtr(unsigned int rows, unsigned int cols, double *ptr, bool row_major=true)
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
LinearSolverLast
RBDL_DLLAPI Matrix3d rotz(const double &zrot)
RBDL_DLLAPI Vector3d global_angular_velocity_from_rates(const Vector3d &zyx_angles, const Vector3d &zyx_rates)
RBDL_DLLAPI VectorNd VectorFromPtr(unsigned int n, double *ptr)
LinearSolverLLT
RBDL_DLLAPI bool SpatialVectorCompareEpsilon(const SpatialVector &vector_a, const SpatialVector &vector_b, double epsilon)
LinearSolverColPivHouseholderQR
RBDL_DLLAPI Matrix3d rotydot(const double &y, const double &ydot)
RBDL_DLLAPI Matrix3d rotxdot(const double &x, const double &xdot)
RBDL_DLLAPI Matrix3d roty(const double &yrot)
RBDL_DLLAPI Matrix3d rotzdot(const double &z, const double &zdot)
RBDL_DLLAPI void SparseMultiplyHx(Model &model, Math::MatrixNd &L)
LinearSolverUnknown
RBDL_DLLAPI void SparseSolveLx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
RBDL_DLLAPI Vector3d angular_acceleration_from_angle_rates(const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot)
RBDL_DLLAPI Matrix3d Matrix3dZero
RBDL_DLLAPI Vector3d angular_velocity_from_angle_rates(const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates)
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)