SpatialAlgebraOperators.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_SPATIALALGEBRAOPERATORS_H__
#define __RDL_SPATIALALGEBRAOPERATORS_H__

#include <iostream>
#include <cmath>
#include <rdl_dynamics/Quaternion.h>
#include "rdl_dynamics/rdl_eigenmath.h"

namespace RobotDynamics
{
namespace Math
{
static inline Vector3d toIntrinsicZYXAngles(const Quaternion& q, double yaw_at_pitch_singularity = 0.)
{
    double sqw = q.w() * q.w();
    double sqx = q.x() * q.x();
    double sqy = q.y() * q.y();
    double sqz = q.z() * q.z();
    double unit = sqx + sqy + sqz + sqw;  // if normalised is one, otherwise is correction factor
    double test = q.w() * q.y() - q.z() * q.x();
    if (test > 0.499 * unit)
    {  // singularity at north pole
        return Vector3d(yaw_at_pitch_singularity, M_PI_2, 2. * atan2(q.x(), q.y()) + yaw_at_pitch_singularity);
    }
    if (test < -0.499 * unit)
    {  // singularity at south pole
        return Vector3d(yaw_at_pitch_singularity, -M_PI_2, 2. * atan2(q.x(), q.w()) - yaw_at_pitch_singularity);
    }

    return Vector3d(atan2(2. * (q.w() * q.z() + q.x() * q.y()), 1. - 2. * (sqy + sqz)), asin(2. * test / unit),
                    atan2(2. * (q.w() * q.x() + q.y() * q.z()), 1. - 2. * (sqx + sqy)));
}

static inline Quaternion toQuaternion(const Vector3d& axis, double angle_rad)
{
    double d = axis.norm();
    double s2 = std::sin(angle_rad * 0.5) / d;

    return Quaternion(axis[0] * s2, axis[1] * s2, axis[2] * s2, std::cos(angle_rad * 0.5));
}

static inline Quaternion toQuaternion(const AxisAngle& axisAngle)
{
    return toQuaternion(axisAngle.axis(), axisAngle.angle());
}

static inline Quaternion intrinsicZYXAnglesToQuaternion(double yaw, double pitch, double roll)
{
    return toQuaternion(Vector3d(0., 0., 1.), yaw) * toQuaternion(Vector3d(0., 1., 0.), pitch) * toQuaternion(Vector3d(1., 0., 0.), roll);
}

static inline Quaternion intrinsicZYXAnglesToQuaternion(const Vector3d& zyx_angles)
{
    return intrinsicZYXAnglesToQuaternion(zyx_angles[0], zyx_angles[1], zyx_angles[2]);
}

static inline Quaternion intrinsicYXZAnglesToQuaternion(double pitch, double roll, double yaw)
{
    return toQuaternion(Vector3d(0., 1., 0.), pitch) * toQuaternion(Vector3d(1., 0., 0.), roll) * toQuaternion(Vector3d(0., 0., 1.), yaw);
}

static inline Quaternion intrinsicYXZAnglesToQuaternion(const Vector3d& yxz_angles)
{
    return intrinsicYXZAnglesToQuaternion(yxz_angles[0], yxz_angles[1], yxz_angles[2]);
}

static inline Quaternion intrinsicXYZAnglesToQuaternion(double roll, double pitch, double yaw)
{
    return toQuaternion(Vector3d(1., 0., 0.), roll) * toQuaternion(Vector3d(0., 1., 0.), pitch) * toQuaternion(Vector3d(0., 0., 1.), yaw);
}

static inline Quaternion intrinsicXYZAnglesToQuaternion(const Vector3d& xyz_angles)
{
    return intrinsicXYZAnglesToQuaternion(xyz_angles[0], xyz_angles[1], xyz_angles[2]);
}

static inline AxisAngle toAxisAngle(Quaternion q)
{
    double n = q.vec().norm();
    if (n < std::numeric_limits<double>::epsilon())
    {
        n = q.vec().stableNorm();
    }

    AxisAngle axisAngle;
    if (n > 0.0)
    {
        axisAngle.angle() = 2.0 * atan2(n, q.w());
        axisAngle.axis() = q.vec() / n;
    }
    else
    {
        axisAngle.angle() = 0.0;
        axisAngle.axis() << 1.0, 0.0, 0.0;
    }

    return axisAngle;
}

static inline Quaternion toQuaternion(const Matrix3d& mat)
{
    double trace = mat.trace();

    if (trace > 0.)
    {
        double s = 2. * std::sqrt(trace + 1.);
        return Quaternion((mat(1, 2) - mat(2, 1)) / s, (mat(2, 0) - mat(0, 2)) / s, (mat(0, 1) - mat(1, 0)) / s, 0.25 * s);
    }
    else if ((mat(0, 0) > mat(1, 1)) && (mat(0, 0) > mat(2, 2)))
    {
        double s = 2. * std::sqrt(1. + mat(0, 0) - mat(1, 1) - mat(2, 2));
        return Quaternion(-0.25 * s, (-mat(0, 1) - mat(1, 0)) / s, (-mat(0, 2) - mat(2, 0)) / s, (mat(2, 1) - mat(1, 2)) / s);
    }
    else if (mat(1, 1) > mat(2, 2))
    {
        double s = 2. * std::sqrt(1. + mat(1, 1) - mat(0, 0) - mat(2, 2));
        return Quaternion((-mat(0, 1) - mat(1, 0)) / s, -0.25 * s, (-mat(1, 2) - mat(2, 1)) / s, (mat(0, 2) - mat(2, 0)) / s);
    }
    else
    {
        double s = 2. * std::sqrt(1. + mat(2, 2) - mat(0, 0) - mat(1, 1));
        return Quaternion((-mat(0, 2) - mat(2, 0)) / s, (-mat(1, 2) - mat(2, 1)) / s, -0.25 * s, (mat(1, 0) - mat(0, 1)) / s);
    }
}

static inline Matrix3d toMatrix(const Quaternion& q)
{
    double x = q.x();
    double y = q.y();
    double z = q.z();
    double w = q.w();

    return Matrix3d(1. - 2. * (y * y + z * z), 2. * x * y + 2. * w * z, 2. * x * z - 2. * w * y, 2. * x * y - 2. * w * z, 1. - 2. * (x * x + z * z),
                    2. * y * z + 2. * w * x, 2. * x * z + 2. * w * y, 2. * y * z - 2. * w * x, 1. - 2. * (x * x + y * y));
}

static inline Vector3d toIntrinsicZYXAngles(const Matrix3d& m, double yaw_at_pitch_singularity = 0.)
{
    // // Assuming the angles are in radians.
    if (m(0, 2) < -0.999)
    {  // singularity at north pole
        return Vector3d(yaw_at_pitch_singularity, M_PI_2, yaw_at_pitch_singularity + atan2(m(1, 0), m(1, 1)));
    }
    if (m(0, 2) > 0.999)
    {  // singularity at south pole
        return Vector3d(yaw_at_pitch_singularity, -M_PI_2, -yaw_at_pitch_singularity + atan2(-m(1, 0), m(1, 1)));
    }

    return Vector3d(atan2(m(0, 1), m(0, 0)), asin(-m(0, 2)), atan2(m(1, 2), m(2, 2)));
}

static inline Matrix3d toTildeForm(const Vector3d& vector)
{
    return Matrix3d(0., -vector[2], vector[1], vector[2], 0., -vector[0], -vector[1], vector[0], 0.);
}

static inline Matrix3d toTildeForm(const double x, const double y, const double z)
{
    return Matrix3d(0., -z, y, z, 0., -x, -y, z, 0.);
}

inline std::ostream& operator<<(std::ostream& output, const SpatialTransform& X)
{
    output << "X.E = " << std::endl << X.E << std::endl;
    output << "X.r = " << X.r.transpose();
    return output;
}

inline SpatialTransform Xrot(double angle_rad, const Vector3d& axis)
{
    double s, c;

    s = sin(angle_rad);
    c = cos(angle_rad);

    return SpatialTransform(Matrix3d(axis[0] * axis[0] * (1.0f - c) + c, axis[1] * axis[0] * (1.0f - c) + axis[2] * s, axis[0] * axis[2] * (1.0f - c) - axis[1] * s,

                                     axis[0] * axis[1] * (1.0f - c) - axis[2] * s, axis[1] * axis[1] * (1.0f - c) + c, axis[1] * axis[2] * (1.0f - c) + axis[0] * s,

                                     axis[0] * axis[2] * (1.0f - c) + axis[1] * s, axis[1] * axis[2] * (1.0f - c) - axis[0] * s, axis[2] * axis[2] * (1.0f - c) + c

                                     ),
                            Vector3d(0., 0., 0.));
}

inline SpatialTransform Xrotx(const double& xrot)
{
    double s, c;

    s = sin(xrot);
    c = cos(xrot);
    return SpatialTransform(Matrix3d(1., 0., 0., 0., c, s, 0., -s, c), Vector3d(0., 0., 0.));
}

inline SpatialTransform Xroty(const double& yrot)
{
    double s, c;

    s = sin(yrot);
    c = cos(yrot);
    return SpatialTransform(Matrix3d(c, 0., -s, 0., 1., 0., s, 0., c), Vector3d(0., 0., 0.));
}

inline SpatialTransform Xrotz(const double& zrot)
{
    double s, c;

    s = sin(zrot);
    c = cos(zrot);
    return SpatialTransform(Matrix3d(c, s, 0., -s, c, 0., 0., 0., 1.), Vector3d(0., 0., 0.));
}

inline SpatialTransform XeulerZYX(double yaw, double pitch, double roll)
{
    return Xrotx(roll) * Xroty(pitch) * Xrotz(yaw);
}

inline SpatialTransform XeulerZYX(const Vector3d& ypr)
{
    return XeulerZYX(ypr[0], ypr[1], ypr[2]);
}

inline SpatialTransform XeulerXYZ(double roll, double pitch, double yaw)
{
    return Xrotz(yaw) * Xroty(pitch) * Xrotx(roll);
}

inline SpatialTransform XeulerXYZ(const Vector3d& xyz_angles)
{
    return XeulerXYZ(xyz_angles[0], xyz_angles[1], xyz_angles[2]);
}

inline SpatialTransform XrotQuat(double x, double y, double z, double w)
{
    return SpatialTransform(toMatrix(Quaternion(x, y, z, w)));
}

inline SpatialTransform XrotQuat(const Quaternion& orientation)
{
    return SpatialTransform(toMatrix(orientation));
}

inline SpatialTransform Xtrans(const Vector3d& r)
{
    return SpatialTransform(Matrix3d::Identity(3, 3), r);
}

inline SpatialMatrix crossm(const SpatialVector& v)
{
    return SpatialMatrix(0, -v[2], v[1], 0, 0, 0, v[2], 0, -v[0], 0, 0, 0, -v[1], v[0], 0, 0, 0, 0, 0, -v[5], v[4], 0, -v[2], v[1], v[5], 0, -v[3], v[2], 0, -v[0], -v[4],
                         v[3], 0, -v[1], v[0], 0);
}

inline SpatialVector crossm(const SpatialVector& v1, const SpatialVector& v2)
{
    return SpatialVector(-v1[2] * v2[1] + v1[1] * v2[2], v1[2] * v2[0] - v1[0] * v2[2], -v1[1] * v2[0] + v1[0] * v2[1],
                         -v1[5] * v2[1] + v1[4] * v2[2] - v1[2] * v2[4] + v1[1] * v2[5], v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5],
                         -v1[4] * v2[0] + v1[3] * v2[1] - v1[1] * v2[3] + v1[0] * v2[4]);
}

inline SpatialMatrix crossf(const SpatialVector& v)
{
    return SpatialMatrix(0, -v[2], v[1], 0, -v[5], v[4], v[2], 0, -v[0], v[5], 0, -v[3], -v[1], v[0], 0, -v[4], v[3], 0, 0, 0, 0, 0, -v[2], v[1], 0, 0, 0, v[2], 0, -v[0],
                         0, 0, 0, -v[1], v[0], 0);
}

inline SpatialVector crossf(const SpatialVector& v1, const SpatialVector& v2)
{
    return SpatialVector(-v1[2] * v2[1] + v1[1] * v2[2] - v1[5] * v2[4] + v1[4] * v2[5], v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5],
                         -v1[1] * v2[0] + v1[0] * v2[1] - v1[4] * v2[3] + v1[3] * v2[4], -v1[2] * v2[4] + v1[1] * v2[5], +v1[2] * v2[3] - v1[0] * v2[5],
                         -v1[1] * v2[3] + v1[0] * v2[4]);
}

inline Vector3d getLinearPartTransformed(const SpatialVector& v, const SpatialTransform& X)
{
    double v_rxw_x = v[3] - X.r[1] * v[2] + X.r[2] * v[1];
    double v_rxw_y = v[4] - X.r[2] * v[0] + X.r[0] * v[2];
    double v_rxw_z = v[5] - X.r[0] * v[1] + X.r[1] * v[0];

    return Vector3d(X.E(0, 0) * v_rxw_x + X.E(0, 1) * v_rxw_y + X.E(0, 2) * v_rxw_z, X.E(1, 0) * v_rxw_x + X.E(1, 1) * v_rxw_y + X.E(1, 2) * v_rxw_z,
                    X.E(2, 0) * v_rxw_x + X.E(2, 1) * v_rxw_y + X.E(2, 2) * v_rxw_z);
}
}  // namespace Math
}  // namespace RobotDynamics

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::SpatialTransform)

/* __RDL_SPATIALALGEBRAOPERATORS_H__*/
#endif  // ifndef __RDL_SPATIALALGEBRAOPERATORS_H__