Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #ifndef __RDL_MATHUTILS_H__
7 : : #define __RDL_MATHUTILS_H__
8 : :
9 : : /**
10 : : * @file rdl_mathutils.hpp
11 : : */
12 : :
13 : : #include <assert.h>
14 : : #include <cmath>
15 : :
16 : : #include "rdl_dynamics/rdl_eigenmath.hpp"
17 : :
18 : : namespace RobotDynamics
19 : : {
20 : : struct Model;
21 : :
22 : : namespace Math
23 : : {
24 : : /**
25 : : * @enum LinearSolver
26 : : * @brief Available solver methods for the linear systems.
27 : : */
28 : : enum LinearSolver
29 : : {
30 : : LinearSolverUnknown = 0,
31 : : LinearSolverPartialPivLU,
32 : : LinearSolverColPivHouseholderQR,
33 : : LinearSolverHouseholderQR,
34 : : LinearSolverLLT,
35 : : LinearSolverLast,
36 : : };
37 : :
38 : : extern Vector3d Vector3dZero;
39 : : extern Matrix3d Matrix3dIdentity;
40 : : extern Matrix3d Matrix3dZero;
41 : : extern Quaternion QuaternionIdentity;
42 : :
43 : : extern SpatialVector SpatialVectorZero;
44 : : extern SpatialMatrix SpatialMatrixIdentity;
45 : : extern SpatialMatrix SpatialMatrixZero;
46 : :
47 : : inline VectorNd vectorFromPtr(unsigned int n, double* ptr)
48 : : {
49 : : // TODO: use memory mapping operators for Eigen
50 : : VectorNd result(n);
51 : :
52 : : for (unsigned int i = 0; i < n; i++)
53 : : {
54 : : result[i] = ptr[i];
55 : : }
56 : :
57 : : return result;
58 : : }
59 : :
60 : : inline MatrixNd matrixFromPtr(unsigned int rows, unsigned int cols, double* ptr, bool row_major = true)
61 : : {
62 : : MatrixNd result(rows, cols);
63 : :
64 : : if (row_major)
65 : : {
66 : : for (unsigned int i = 0; i < rows; i++)
67 : : {
68 : : for (unsigned int j = 0; j < cols; j++)
69 : : {
70 : : result(i, j) = ptr[i * cols + j];
71 : : }
72 : : }
73 : : }
74 : : else
75 : : {
76 : : for (unsigned int i = 0; i < rows; i++)
77 : : {
78 : : for (unsigned int j = 0; j < cols; j++)
79 : : {
80 : : result(i, j) = ptr[i + j * rows];
81 : : }
82 : : }
83 : : }
84 : :
85 : : return result;
86 : : }
87 : :
88 : : /// \brief Solves a linear system using gaussian elimination with pivoting
89 : : bool linSolveGaussElimPivot(MatrixNd A, VectorNd b, VectorNd& x);
90 : :
91 : : /** \brief Translates the inertia matrix to a new center. */
92 : : Matrix3d parallel_axis(const Matrix3d& inertia, double mass, const Vector3d& com);
93 : :
94 : : /**
95 : : * @brief convert zyx euler angles and angle rates to angular velocity
96 : : * @param xyz_angles (yaw, pitch, roll) euler angles
97 : : * @param xyz_angle_rates (yaw_dot, pitch_dot, roll_dot)
98 : : * @return angular velocities (wx, wy, wz)
99 : : */
100 : 8 : inline Vector3d angular_velocity_from_zyx_angle_rates(const Vector3d& zyx_angles, const Vector3d& zyx_angle_rates)
101 : : {
102 : 8 : double sy = sin(zyx_angles[1]);
103 : 8 : double cy = cos(zyx_angles[1]);
104 : 8 : double sx = sin(zyx_angles[2]);
105 : 8 : double cx = cos(zyx_angles[2]);
106 : :
107 : 8 : return Vector3d(zyx_angle_rates[2] - sy * zyx_angle_rates[0], cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0],
108 : 24 : -sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0]);
109 : : }
110 : :
111 : : /**
112 : : * @brief convert xyz euler angles and angle rates to angular velocity
113 : : * @param xyz_angles (roll, pitch, yaw) euler angles
114 : : * @param xyz_angle_rates (roll_dot, pitch_dot, yaw_dot)
115 : : * @return angular velocities (wx, wy, wz)
116 : : */
117 : 2 : inline Vector3d angular_velocity_from_xyz_angle_rates(const Vector3d& xyz_angles, const Vector3d& xyz_angle_rates)
118 : : {
119 : 2 : double s1 = sin(xyz_angles[1]);
120 : 2 : double c1 = cos(xyz_angles[1]);
121 : 2 : double s2 = sin(xyz_angles[2]);
122 : 2 : double c2 = cos(xyz_angles[2]);
123 : :
124 : 2 : return Vector3d(c2 * c1 * xyz_angle_rates[0] + s2 * xyz_angle_rates[1], -s2 * c1 * xyz_angle_rates[0] + c2 * xyz_angle_rates[1],
125 : 6 : s1 * xyz_angle_rates[0] + xyz_angle_rates[2]);
126 : : }
127 : :
128 : : /**
129 : : * @brief compute the 3x3 jacobian matrix that when multiplied by the angular velocity, ordered (wx, wy, wz) returns the angular velocity vector, note that
130 : : * the ordering of the returned angle rates is (yaw_dot, pitch_Dot, roll_dot)
131 : : * @param zyx_angles yaw/pitch/roll euler angles, ordered (yaw, pitch, roll)
132 : : * @param singularity threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and
133 : : * set errno to EDOM, signifying a domain error
134 : : * @return jacobian matrix, S, that when multiplied by angular velocity, v as in S*v returns a vector of angle rates
135 : : */
136 : 1 : inline Matrix3d angular_velocity_to_zyx_angle_rates_jacobian(const Vector3d& zyx_angles, double singularity_threshold = 1.e-10)
137 : : {
138 : 1 : double c1 = cos(zyx_angles[1]);
139 : 1 : double t1 = tan(zyx_angles[1]);
140 : 1 : double s2 = sin(zyx_angles[2]);
141 : 1 : double c2 = cos(zyx_angles[2]);
142 : :
143 : 1 : if (c1 < singularity_threshold)
144 : : {
145 : 0 : errno = EDOM;
146 : 0 : std::cerr << "Domain error - unable to convert angular velocities to ZYX euler rates with pitch = pi/2" << std::endl;
147 : 0 : return Matrix3dZero;
148 : : }
149 : :
150 : 2 : return Matrix3d(0., s2 / c1, c2 / c1, 0., c2, -s2, 1., s2 * t1, c2 * t1);
151 : : }
152 : :
153 : : /**
154 : : * @brief compute the 3x3 jacobian matrix that when multiplied by the angular velocity, ordered (wx, wy, wz) returns the angular velocity vector, note that
155 : : * the ordering of the returned angle rates is (roll_dot, pitch_dot, yaw_dot)
156 : : * @param xyz_angles roll/pitch/yaw euler angles, ordered (roll, pitch, yaw)
157 : : * @param singularity threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and
158 : : * set errno to EDOM, signifying a domain error
159 : : * @return jacobian matrix, S, that when multiplied by angular velocity, v as in S*v returns a vector of angle rates
160 : : */
161 : 1 : inline Matrix3d angular_velocity_to_xyz_angle_rates_jacobian(const Vector3d& xyz_angles, double singularity_threshold = 1.e-10)
162 : : {
163 : 1 : double c1 = cos(xyz_angles[1]);
164 : 1 : double t1 = tan(xyz_angles[1]);
165 : 1 : double s2 = sin(xyz_angles[2]);
166 : 1 : double c2 = cos(xyz_angles[2]);
167 : :
168 : 1 : if (c1 < singularity_threshold)
169 : : {
170 : 0 : errno = EDOM;
171 : 0 : std::cerr << "Domain error - unable to convert angular velocities to XYZ euler rates with pitch = pi/2" << std::endl;
172 : 0 : return Matrix3dZero;
173 : : }
174 : :
175 : 2 : return Matrix3d(c2 / c1, -s2 / c1, 0., s2, c2, 0., -c2 * t1, s2 * t1, 1.);
176 : : }
177 : :
178 : : /**
179 : : * @brief convert angular velocity to zyx angle rates
180 : : * @param zyx_angles yaw/pitch/roll euler angles, ordered (yaw, pitch, roll)
181 : : * @param angular_velocity angular velocity (x, y, z)
182 : : * @param singularity threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a matrix of zeros and
183 : : * set errno to EDOM, signifying a domain error
184 : : * @return given zyx angles and angular velocity, a vector of angle rates orderd (yaw_dot, pitch_dot, roll_dot)
185 : : */
186 : 3 : inline Vector3d angular_velocity_to_zyx_angle_rates(const Vector3d& zyx_angles, const Vector3d& angular_velocity, double singularity_threshold = 1.e-10)
187 : : {
188 : 3 : double c1 = cos(zyx_angles[1]);
189 : 3 : double t1 = tan(zyx_angles[1]);
190 : 3 : double s2 = sin(zyx_angles[2]);
191 : 3 : double c2 = cos(zyx_angles[2]);
192 : :
193 : 3 : if (c1 < singularity_threshold)
194 : : {
195 : 0 : errno = EDOM;
196 : 0 : std::cerr << "unable to compute angle rates from angular velocity due to singularity" << std::endl;
197 : 0 : return Vector3dZero;
198 : : }
199 : :
200 : 3 : return Vector3d((s2 / c1) * angular_velocity[1] + (c2 / c1) * angular_velocity[2], c2 * angular_velocity[1] - s2 * angular_velocity[2],
201 : 9 : angular_velocity[0] + s2 * t1 * angular_velocity[1] + c2 * t1 * angular_velocity[2]);
202 : : }
203 : :
204 : : /**
205 : : * @brief convert angular velocity to xyz angle rates
206 : : * @param xyz_angles yaw/pitch/roll euler angles, ordered (roll, pitch, yaw)
207 : : * @param angular_velocity angular velocity (x, y, z)
208 : : * @param singularity threshold, if the cosine of pitch is less than this, then it is considered at singularity and will return a vector of zeros and
209 : : * set errno to EDOM, signifying a domain error
210 : : * @return given xyz angles and angular velocity, a vector of angle rates orderd (roll_dot, pitch_dot, yaw_dot)
211 : : */
212 : 3 : inline Vector3d angular_velocity_to_xyz_angle_rates(const Vector3d& xyz_angles, const Vector3d& angular_velocity, double singularity_threshold = 1.e-10)
213 : : {
214 : 3 : double c1 = cos(xyz_angles[1]);
215 : 3 : double t1 = tan(xyz_angles[1]);
216 : 3 : double s2 = sin(xyz_angles[2]);
217 : 3 : double c2 = cos(xyz_angles[2]);
218 : :
219 : 3 : if (c1 < singularity_threshold)
220 : : {
221 : 0 : errno = EDOM;
222 : 0 : std::cerr << "unable to compute jacobian matrix for XYZ angles" << std::endl;
223 : 0 : return Vector3dZero;
224 : : }
225 : :
226 : 3 : return Vector3d((c2 / c1) * angular_velocity[0] - (s2 / c1) * angular_velocity[1], s2 * angular_velocity[0] + c2 * angular_velocity[1],
227 : 9 : -c2 * t1 * angular_velocity[0] + s2 * t1 * angular_velocity[1] + angular_velocity[2]);
228 : : }
229 : :
230 : 1 : inline Vector3d global_angular_velocity_from_rates(const Vector3d& zyx_angles, const Vector3d& zyx_rates)
231 : : {
232 : : double s0, c0, s1, c1;
233 : 1 : s0 = sin(zyx_angles[0]);
234 : 1 : c0 = cos(zyx_angles[0]);
235 : 1 : s1 = sin(zyx_angles[1]);
236 : 1 : c1 = cos(zyx_angles[1]);
237 : :
238 : 1 : Matrix3d RzT(c0, s0, 0., -s0, c0, 0., 0., 0., 1.);
239 : 1 : RzT.transposeInPlace();
240 : 1 : Matrix3d RyT(c1, 0., -s1, 0., 1., 0., s1, 0., c1);
241 : 1 : RyT.transposeInPlace();
242 : :
243 : 2 : return Vector3d(Vector3d(0., 0., zyx_rates[0]) + RzT * Vector3d(0., zyx_rates[1], 0.) + RzT * RyT * Vector3d(zyx_rates[2], 0., 0.));
244 : : }
245 : :
246 : : /**
247 : : * @brief calculate angular acceleration from zyx angles, angle rates, and angle accelerations
248 : : * @param zyx_angles (yaw, pitch, roll) euler angles
249 : : * @param zyx_angle_rates (yaw_dot, pitch_dot, roll_dot) angle rates
250 : : * @param zyx_angle_accelerations (yaw_ddot, pitch_ddot, roll_ddot) angle accelerations
251 : : *
252 : : * @return angular accelerations (wx_dot, wy_dot, wz_dot)
253 : : */
254 : 1 : inline Vector3d angular_acceleration_from_zyx_angle_rates(const Vector3d& zyx_angles, const Vector3d& zyx_angle_rates, const Vector3d& zyx_angle_rates_dot)
255 : : {
256 : 1 : double sy = sin(zyx_angles[1]);
257 : 1 : double cy = cos(zyx_angles[1]);
258 : 1 : double sx = sin(zyx_angles[2]);
259 : 1 : double cx = cos(zyx_angles[2]);
260 : 1 : double xdot = zyx_angle_rates[2];
261 : 1 : double ydot = zyx_angle_rates[1];
262 : 1 : double zdot = zyx_angle_rates[0];
263 : 1 : double xddot = zyx_angle_rates_dot[2];
264 : 1 : double yddot = zyx_angle_rates_dot[1];
265 : 1 : double zddot = zyx_angle_rates_dot[0];
266 : :
267 : 0 : return Vector3d(xddot - (cy * ydot * zdot + sy * zddot), -sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * (-sy * ydot * zdot + cy * zddot),
268 : 2 : -cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * (-sy * ydot * zdot + cy * zddot));
269 : : }
270 : :
271 : : /**
272 : : * @brief calculate angular acceleration from zyx angles, angle rates, and angle accelerations
273 : : * @param xyz_angles (roll, pitch, yaw) euler angles
274 : : * @param xyz_angle_rates (roll_dot, pitch_dot, yaw_dot) angle rates
275 : : * @param xyz_angle_accelerations (roll_ddot, pitch_ddot, yaw_ddot) angle accelerations
276 : : *
277 : : * @return angular accelerations (wx_dot, wy_dot, wz_dot)
278 : : */
279 : 1 : inline Vector3d angular_acceleration_from_xyz_angle_rates(const Vector3d& xyz_angles, const Vector3d& xyz_angle_rates, const Vector3d& xyz_angle_rates_dot)
280 : : {
281 : 1 : double s1 = sin(xyz_angles[1]);
282 : 1 : double c1 = cos(xyz_angles[1]);
283 : 1 : double s2 = sin(xyz_angles[2]);
284 : 1 : double c2 = cos(xyz_angles[2]);
285 : :
286 : 1 : double xdot = xyz_angle_rates[0];
287 : 1 : double ydot = xyz_angle_rates[1];
288 : 1 : double zdot = xyz_angle_rates[2];
289 : :
290 : 1 : double xddot = xyz_angle_rates_dot[0];
291 : 1 : double yddot = xyz_angle_rates_dot[1];
292 : 1 : double zddot = xyz_angle_rates_dot[2];
293 : 0 : return Vector3d(c2 * c1 * xddot + s2 * yddot + (-s2 * c1 * zdot - c2 * s1 * ydot) * xdot + c2 * zdot * ydot,
294 : 2 : -s2 * c1 * xddot + c2 * yddot + (-c2 * c1 * zdot + s2 * s1 * ydot) * xdot - s2 * zdot * ydot, s1 * xddot + zddot + c1 * ydot * xdot);
295 : : }
296 : :
297 : : void SparseFactorizeLTL(Model& model, Math::MatrixNd& H);
298 : :
299 : : void SparseMultiplyHx(Model& model, Math::MatrixNd& L);
300 : :
301 : : void SparseMultiplyLx(Model& model, Math::MatrixNd& L);
302 : :
303 : : void SparseMultiplyLTx(Model& model, Math::MatrixNd& L);
304 : :
305 : : void SparseSolveLx(Model& model, Math::MatrixNd& L, Math::VectorNd& x);
306 : :
307 : : void SparseSolveLTx(Model& model, Math::MatrixNd& L, Math::VectorNd& x);
308 : : } // namespace Math
309 : : } // namespace RobotDynamics
310 : :
311 : : /* __RDL_MATHUTILS_H__ */
312 : : #endif // ifndef __RDL_MATHUTILS_H__
|
