protot/3rdparty/rbdl/src/rbdl_mathutils.cc

320 lines
8.0 KiB
C++
Raw Normal View History

/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2016 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#include <cmath>
#include <limits>
#include <iostream>
#include <assert.h>
#include <rbdl/rbdl_mathutils.h>
#include <rbdl/Model.h>
#include "rbdl/Logging.h"
namespace RigidBodyDynamics {
namespace Math {
RBDL_DLLAPI Vector3d Vector3dZero (0., 0., 0.);
RBDL_DLLAPI Matrix3d Matrix3dIdentity (
1., 0., 0.,
0., 1., 0.,
0., 0., 1
);
RBDL_DLLAPI Matrix3d Matrix3dZero (
0., 0., 0.,
0., 0., 0.,
0., 0., 0.
);
RBDL_DLLAPI SpatialVector SpatialVectorZero ( 0., 0., 0., 0., 0., 0.);
RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) {
x = VectorNd::Zero(x.size());
// We can only solve quadratic systems
assert (A.rows() == A.cols());
unsigned int n = A.rows();
unsigned int pi;
// the pivots
size_t *pivot = new size_t[n];
// temporary result vector which contains the pivoted result
VectorNd px(x);
unsigned int i,j,k;
for (i = 0; i < n; i++)
pivot[i] = i;
for (j = 0; j < n; j++) {
pi = j;
double pv = fabs (A(j,pivot[j]));
// LOG << "j = " << j << " pv = " << pv << std::endl;
// find the pivot
for (k = j; k < n; k++) {
double pt = fabs (A(j,pivot[k]));
if (pt > pv) {
pv = pt;
pi = k;
unsigned int p_swap = pivot[j];
pivot[j] = pivot[pi];
pivot[pi] = p_swap;
// LOG << "swap " << j << " with " << pi << std::endl;
// LOG << "j = " << j << " pv = " << pv << std::endl;
}
}
for (i = j + 1; i < n; i++) {
if (fabs(A(j,pivot[j])) <= std::numeric_limits<double>::epsilon()) {
std::cerr << "Error: pivoting failed for matrix A = " << std::endl;
std::cerr << "A = " << std::endl << A << std::endl;
std::cerr << "b = " << b << std::endl;
}
// assert (fabs(A(j,pivot[j])) > std::numeric_limits<double>::epsilon());
double d = A(i,pivot[j])/A(j,pivot[j]);
b[i] -= b[j] * d;
for (k = j; k < n; k++) {
A(i,pivot[k]) -= A(j,pivot[k]) * d;
}
}
}
// warning: i is an unsigned int, therefore a for loop of the
// form "for (i = n - 1; i >= 0; i--)" might end up in getting an invalid
// value for i!
i = n;
do {
i--;
for (j = i + 1; j < n; j++) {
px[i] += A(i,pivot[j]) * px[j];
}
px[i] = (b[i] - px[i]) / A(i,pivot[i]);
} while (i > 0);
// Unswapping
for (i = 0; i < n; i++) {
x[pivot[i]] = px[i];
}
/*
LOG << "A = " << std::endl << A << std::endl;
LOG << "b = " << b << std::endl;
LOG << "x = " << x << std::endl;
LOG << "pivot = " << pivot[0] << " " << pivot[1] << " " << pivot[2] << std::endl;
std::cout << LogOutput.str() << std::endl;
*/
delete[] pivot;
return true;
}
RBDL_DLLAPI void SpatialMatrixSetSubmatrix(
SpatialMatrix &dest,
unsigned int row,
unsigned int col,
const Matrix3d &matrix) {
assert (row < 2 && col < 2);
dest(row*3,col*3) = matrix(0,0);
dest(row*3,col*3 + 1) = matrix(0,1);
dest(row*3,col*3 + 2) = matrix(0,2);
dest(row*3 + 1,col*3) = matrix(1,0);
dest(row*3 + 1,col*3 + 1) = matrix(1,1);
dest(row*3 + 1,col*3 + 2) = matrix(1,2);
dest(row*3 + 2,col*3) = matrix(2,0);
dest(row*3 + 2,col*3 + 1) = matrix(2,1);
dest(row*3 + 2,col*3 + 2) = matrix(2,2);
}
RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (
const SpatialMatrix &matrix_a,
const SpatialMatrix &matrix_b,
double epsilon) {
assert (epsilon >= 0.);
unsigned int i, j;
for (i = 0; i < 6; i++) {
for (j = 0; j < 6; j++) {
if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) {
std::cerr << "Expected:"
<< std::endl << matrix_a << std::endl
<< "but was" << std::endl
<< matrix_b << std::endl;
return false;
}
}
}
return true;
}
RBDL_DLLAPI bool SpatialVectorCompareEpsilon (
const SpatialVector &vector_a,
const SpatialVector &vector_b,
double epsilon) {
assert (epsilon >= 0.);
unsigned int i;
for (i = 0; i < 6; i++) {
if (fabs(vector_a[i] - vector_b[i]) >= epsilon) {
std::cerr << "Expected:"
<< std::endl << vector_a << std::endl
<< "but was" << std::endl
<< vector_b << std::endl;
return false;
}
}
return true;
}
RBDL_DLLAPI Matrix3d parallel_axis (
const Matrix3d &inertia,
double mass,
const Vector3d &com) {
Matrix3d com_cross = VectorCrossMatrix (com);
return inertia + mass * com_cross * com_cross.transpose();
}
RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &r) {
return SpatialMatrix (
1., 0., 0., 0., 0., 0.,
0., 1., 0., 0., 0., 0.,
0., 0., 1., 0., 0., 0.,
0., r[2], -r[1], 1., 0., 0.,
-r[2], 0., r[0], 0., 1., 0.,
r[1], -r[0], 0., 0., 0., 1.
);
}
RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) {
double s, c;
s = sin (xrot);
c = cos (xrot);
return SpatialMatrix(
1., 0., 0., 0., 0., 0.,
0., c, s, 0., 0., 0.,
0., -s, c, 0., 0., 0.,
0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., c, s,
0., 0., 0., 0., -s, c
);
}
RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) {
double s, c;
s = sin (yrot);
c = cos (yrot);
return SpatialMatrix(
c, 0., -s, 0., 0., 0.,
0., 1., 0., 0., 0., 0.,
s, 0., c, 0., 0., 0.,
0., 0., 0., c, 0., -s,
0., 0., 0., 0., 1., 0.,
0., 0., 0., s, 0., c
);
}
RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot) {
double s, c;
s = sin (zrot);
c = cos (zrot);
return SpatialMatrix(
c, s, 0., 0., 0., 0.,
-s, c, 0., 0., 0., 0.,
0., 0., 1., 0., 0., 0.,
0., 0., 0., c, s, 0.,
0., 0., 0., -s, c, 0.,
0., 0., 0., 0., 0., 1.
);
}
RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (
const Vector3d &displacement,
const Vector3d &zyx_euler) {
return Xrotz_mat(zyx_euler[0]) * Xroty_mat(zyx_euler[1]) * Xrotx_mat(zyx_euler[2]) * Xtrans_mat(displacement);
}
RBDL_DLLAPI void SparseFactorizeLTL (Model &model, Math::MatrixNd &H) {
for (unsigned int i = 0; i < model.qdot_size; i++) {
for (unsigned int j = i + 1; j < model.qdot_size; j++) {
H(i,j) = 0.;
}
}
for (unsigned int k = model.qdot_size; k > 0; k--) {
H(k - 1,k - 1) = sqrt (H(k - 1,k - 1));
unsigned int i = model.lambda_q[k];
while (i != 0) {
H(k - 1,i - 1) = H(k - 1,i - 1) / H(k - 1,k - 1);
i = model.lambda_q[i];
}
i = model.lambda_q[k];
while (i != 0) {
unsigned int j = i;
while (j != 0) {
H(i - 1,j - 1) = H(i - 1,j - 1) - H(k - 1,i - 1) * H(k - 1, j - 1);
j = model.lambda_q[j];
}
i = model.lambda_q[i];
}
}
}
RBDL_DLLAPI void SparseMultiplyHx (Model &model, Math::MatrixNd &L) {
assert (0 && !"Not yet implemented!");
}
RBDL_DLLAPI void SparseMultiplyLx (Model &model, Math::MatrixNd &L) {
assert (0 && !"Not yet implemented!");
}
RBDL_DLLAPI void SparseMultiplyLTx (Model &model, Math::MatrixNd &L) {
assert (0 && !"Not yet implemented!");
}
RBDL_DLLAPI void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) {
for (unsigned int i = 1; i <= model.qdot_size; i++) {
unsigned int j = model.lambda_q[i];
while (j != 0) {
x[i - 1] = x[i - 1] - L(i - 1,j - 1) * x[j - 1];
j = model.lambda_q[j];
}
x[i - 1] = x[i - 1] / L(i - 1,i - 1);
}
}
RBDL_DLLAPI void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) {
for (int i = model.qdot_size; i > 0; i--) {
x[i - 1] = x[i - 1] / L(i - 1,i - 1);
unsigned int j = model.lambda_q[i];
while (j != 0) {
x[j - 1] = x[j - 1] - L(i - 1,j - 1) * x[i - 1];
j = model.lambda_q[j];
}
}
}
} /* Math */
} /* RigidBodyDynamics */