rbdl_mathutils.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_MATHUTILS_H
#define RBDL_MATHUTILS_H
 
#include <assert.h>
//#include <cmath>
 
#include "rbdl/rbdl_math.h"
 
namespace RigidBodyDynamics {
struct Model;
 
namespace Math {
 
enum RBDL_DLLAPI LinearSolver {
  LinearSolverUnknown = 0,
  LinearSolverPartialPivLU,
  LinearSolverColPivHouseholderQR,
  LinearSolverHouseholderQR,
  LinearSolverLLT,
  LinearSolverLast,
};
 
extern RBDL_DLLAPI Vector3d Vector3dZero;
extern RBDL_DLLAPI Matrix3d Matrix3dIdentity;
extern RBDL_DLLAPI Matrix3d Matrix3dZero;
 
RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, Scalar *ptr) {
  // TODO: use memory mapping operators for Eigen
  VectorNd result (n);
 
  for (unsigned int i = 0; i < n; i++) {
    result[i] = ptr[i];
  }
 
  return result;
}
 
RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, Scalar *ptr, bool row_major = true) {
  MatrixNd result (rows, cols);
 
  if (row_major) {
    for (unsigned int i = 0; i < rows; i++) {
      for (unsigned int j = 0; j < cols; j++) {
        result(i,j) = ptr[i * cols + j];
      }
    }
  } else {
    for (unsigned int i = 0; i < rows; i++) {
      for (unsigned int j = 0; j < cols; j++) {
        result(i,j) = ptr[i + j * rows];
      }
    }
  }
 
  return result;
}
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
#endif
 
// \todo write test 
RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix);
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a,
    const SpatialMatrix &matrix_b, Scalar epsilon);
RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a,
    const SpatialVector &vector_b, Scalar epsilon);
#endif
 
RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, Scalar mass, const Vector3d &com);
 
RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement);
 
RBDL_DLLAPI SpatialMatrix Xrotz_mat (const Scalar &zrot);
 
RBDL_DLLAPI SpatialMatrix Xroty_mat (const Scalar &yrot);
 
RBDL_DLLAPI SpatialMatrix Xrotx_mat (const Scalar &xrot);
 
RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler);
 
RBDL_DLLAPI inline Matrix3d rotx (const Scalar &xrot) {
  Scalar s, c;
  s = sin (xrot);
  c = cos (xrot);
  return Matrix3d (
      1., 0., 0.,
      0., c, s,
      0., -s, c
      );
}
 
RBDL_DLLAPI inline Matrix3d roty (const Scalar &yrot) {
  Scalar s, c;
  s = sin (yrot);
  c = cos (yrot);
  return Matrix3d (
      c, 0., -s,
      0., 1., 0.,
      s, 0., c
      );
}
 
RBDL_DLLAPI inline Matrix3d rotz (const Scalar &zrot) {
  Scalar s, c;
  s = sin (zrot);
  c = cos (zrot);
  return Matrix3d (
      c, s, 0.,
      -s, c, 0.,
      0., 0., 1.
      );
}
 
RBDL_DLLAPI inline Matrix3d rotxdot (const Scalar &x, const Scalar &xdot) {
  Scalar s, c;
  s = sin (x);
  c = cos (x);
  return Matrix3d (
      0., 0., 0.,
      0., -s * xdot, c * xdot,
      0., -c * xdot,-s * xdot 
      );
}
 
RBDL_DLLAPI inline Matrix3d rotydot (const Scalar &y, const Scalar &ydot) {
  Scalar s, c;
  s = sin (y);
  c = cos (y);
  return Matrix3d (
      -s * ydot, 0., - c * ydot,
      0., 0., 0.,
      c * ydot, 0., - s * ydot 
      );
}
 
RBDL_DLLAPI inline Matrix3d rotzdot (const Scalar &z, const Scalar &zdot) {
  Scalar s, c;
  s = sin (z);
  c = cos (z);
  return Matrix3d (
      -s * zdot, c * zdot, 0.,
      -c * zdot, -s * zdot, 0.,
      0., 0., 0.
      );
}
 
RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) {
  Scalar sy = sin(zyx_angles[1]);
  Scalar cy = cos(zyx_angles[1]);
  Scalar sx = sin(zyx_angles[2]);
  Scalar cx = cos(zyx_angles[2]);
 
  return Vector3d (
      zyx_angle_rates[2] - sy * zyx_angle_rates[0],
      cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0],
      -sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0]
      );
}
 
RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) {
  Matrix3d RzT = rotz(zyx_angles[0]).transpose();
  Matrix3d RyT = roty(zyx_angles[1]).transpose();
 
  return Vector3d (
      Vector3d (0., 0., zyx_rates[0])
      + RzT * Vector3d (0., zyx_rates[1], 0.)
      + RzT * RyT * Vector3d (zyx_rates[2], 0., 0.)
      );
}
 
RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) {
  Scalar sy = sin(zyx_angles[1]);
  Scalar cy = cos(zyx_angles[1]);
  Scalar sx = sin(zyx_angles[2]);
  Scalar cx = cos(zyx_angles[2]);
  Scalar xdot = zyx_angle_rates[2];
  Scalar ydot = zyx_angle_rates[1];
  Scalar zdot = zyx_angle_rates[0];
  Scalar xddot = zyx_angle_rates_dot[2];
  Scalar yddot = zyx_angle_rates_dot[1];
  Scalar zddot = zyx_angle_rates_dot[0];
 
  return Vector3d (
      xddot - (cy * ydot * zdot + sy * zddot),
      -sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot),
      -cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot)
      );
}
 
RBDL_DLLAPI
void SparseFactorizeLTL (Model &model, Math::MatrixNd &H);
 
RBDL_DLLAPI
void SparseMultiplyHx (Model &model, Math::MatrixNd &L);
 
RBDL_DLLAPI
void SparseMultiplyLx (Model &model, Math::MatrixNd &L);
RBDL_DLLAPI
void SparseMultiplyLTx (Model &model, Math::MatrixNd &L);
 
RBDL_DLLAPI
void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x);
RBDL_DLLAPI
void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x); 
 
} /* Math */
 
} /* RigidBodyDynamics */
 
/* RBDL_MATHUTILS_H */
#endif