Math.hpp

Go to the documentation of this file.

#ifndef TR_MATH_HPP
#define TR_MATH_HPP
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace tinyrobotics {
    template <typename Scalar>
    Eigen::Matrix<Scalar, 3, 3> skew(const Eigen::Matrix<Scalar, 3, 3>& input) {
        return 0.5 * (input - input.transpose());
    }
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 3, 3> skew(const Eigen::Matrix<Scalar, 3, 1>& input) {
        Eigen::Matrix<Scalar, 3, 3> out;
        out << Scalar(0), -input(2), input(1), input(2), Scalar(0), -input(0), -input(1), input(0), Scalar(0);
        return out;
    }
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> null(
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& A) {
        Eigen::FullPivLU<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>> lu(A);
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> A_null_space = lu.kernel();
        return A_null_space;
    }
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> cross_spatial(const Eigen::Matrix<Scalar, 6, 1>& v) {
        Eigen::Matrix<Scalar, 6, 6> vcross;
        vcross << 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;
        return vcross;
    }
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> cross_motion(const Eigen::Matrix<Scalar, 6, 1>& v) {
        return -cross_spatial(v).transpose();
    }
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> translation_to_spatial(const Eigen::Matrix<Scalar, 3, 1>& xyz) {
        Eigen::Matrix<Scalar, 6, 6> X = Eigen::Matrix<Scalar, 6, 6>::Identity();
        X.block(3, 0, 3, 3)           = -skew(xyz);
        return X;
    }
 
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> rpy_to_spatial(const Eigen::Matrix<Scalar, 3, 1>& rpy) {
        Eigen::Matrix<Scalar, 3, 3> E = rx(rpy(0)) * ry(rpy(1)) * rz(rpy(2));
        Eigen::Matrix<Scalar, 6, 6> X = Eigen::Matrix<Scalar, 6, 6>::Identity();
        X.block(0, 0, 3, 3)           = E;
        X.block(3, 3, 3, 3)           = E;
        return X;
    }
 
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> homogeneous_to_spatial(const Eigen::Transform<Scalar, 3, Eigen::Isometry>& H) {
        Eigen::Matrix<Scalar, 6, 6> X = Eigen::Matrix<Scalar, 6, 6>::Zero();
        X.block(0, 0, 3, 3)           = H.rotation();
        X.block(3, 3, 3, 3)           = H.rotation();
        X.block(3, 0, 3, 3)           = skew(Eigen::Matrix<Scalar, 3, 1>(H.translation())) * H.rotation();
        return X;
    }
 
 
    template <typename Scalar>
    Eigen::Matrix<Scalar, 6, 6> inertia_to_spatial(const Scalar m,
                                                   const Eigen::Matrix<Scalar, 3, 1>& c,
                                                   const Eigen::Matrix<Scalar, 3, 3>& I) {
        Eigen::Matrix<Scalar, 6, 6> Ic;
        Eigen::Matrix<Scalar, 3, 3> C = skew(c);
        Ic.setZero();
        Ic.block(0, 0, 3, 3) = I + m * C * C.transpose();
        Ic.block(0, 3, 3, 3) = m * C;
        Ic.block(3, 0, 3, 3) = m * C.transpose();
        Ic.block(3, 3, 3, 3) = m * Eigen::Matrix<Scalar, 3, 3>::Identity();
        return Ic;
    }
 
}  // namespace tinyrobotics
 
#endif