RigidBodyInertia.hpp

Go to the documentation of this file.

/*
 * RDL - Robot Dynamics Library
 * Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_RIGID_BODY_INERTIA_HPP__
#define __RDL_RIGID_BODY_INERTIA_HPP__

#include "rdl_dynamics/rdl_eigenmath.h"
#include "rdl_dynamics/SpatialAlgebraOperators.h"

namespace RobotDynamics
{
namespace Math
{
class RigidBodyInertia : public TransformableGeometricObject
{
  public:
    RigidBodyInertia() : m(0.), h(Vector3d::Zero(3, 1)), Ixx(0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.)
    {
    }

    RigidBodyInertia(double mass, const Vector3d& com_mass, const Matrix3d& inertia)
      : m(mass), h(com_mass), Ixx(inertia(0, 0)), Iyx(inertia(1, 0)), Iyy(inertia(1, 1)), Izx(inertia(2, 0)), Izy(inertia(2, 1)), Izz(inertia(2, 2))
    {
    }

    RigidBodyInertia(double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx, const double Izy, const double Izz)
      : m(m), h(h), Ixx(Ixx), Iyx(Iyx), Iyy(Iyy), Izx(Izx), Izy(Izy), Izz(Izz)
    {
    }

    RigidBodyInertia(const RigidBodyInertia& inertia)
      : m(inertia.m), h(inertia.h), Ixx(inertia.Ixx), Iyx(inertia.Iyx), Iyy(inertia.Iyy), Izx(inertia.Izx), Izy(inertia.Izy), Izz(inertia.Izz)
    {
    }

    inline void set(const RigidBodyInertia& I)
    {
        this->set(I.m, I.h, I.Ixx, I.Iyx, I.Iyy, I.Izx, I.Izy, I.Izz);
    }

    void set(double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx, const double Izy, const double Izz)
    {
        this->m = m;
        this->h = h;
        this->Ixx = Ixx;
        this->Iyx = Iyx;
        this->Iyy = Iyy;
        this->Izx = Izx;
        this->Izy = Izy;
        this->Izz = Izz;
    }

    void operator+=(const RigidBodyInertia& rbi);

    void transform(const SpatialTransform& X);

    inline RigidBodyInertia transform_copy(const SpatialTransform& X) const
    {
        RigidBodyInertia I = *this;
        I.transform(X);
        return I;
    }

    void createFromMatrix(const SpatialMatrix& Ic);

    SpatialMatrix toMatrix() const;

    void setSpatialMatrix(SpatialMatrix& mat) const;

    inline SpatialMatrix subtractSpatialMatrix(const SpatialMatrix& m) const
    {
        return SpatialMatrix(Ixx - m(0, 0), Iyx - m(0, 1), Izx - m(0, 2), -m(0, 3), -h[2] - m(0, 4), h[1] - m(0, 5), Iyx - m(1, 0), Iyy - m(1, 1), Izy - m(1, 2),
                             h[2] - m(1, 3), -m(1, 4), -h[0] - m(1, 5), Izx - m(2, 0), Izy - m(2, 1), Izz - m(2, 2), -h[1] - m(2, 3), h[0] - m(2, 4), -m(2, 5), -m(3, 0),
                             h[2] - m(3, 1), -h[1] - m(3, 2), this->m - m(3, 3), -m(3, 4), -m(3, 5), -h[2] - m(4, 0), -m(4, 1), h[0] - m(4, 2), -m(4, 3),
                             this->m - m(4, 4), -m(4, 5), h[1] - m(5, 0), -h[0] - m(5, 1), -m(5, 2), -m(5, 3), -m(5, 4), this->m - m(5, 5));
    }

    inline SpatialVector timesSpatialVector(const SpatialVector& v) const
    {
        return SpatialVector(Ixx * v[0] + Iyx * v[1] + Izx * v[2] + h[1] * v[5] - h[2] * v[4], Iyx * v[0] + Iyy * v[1] + Izy * v[2] - h[0] * v[5] + h[2] * v[3],
                             Izx * v[0] + Izy * v[1] + Izz * v[2] + h[0] * v[4] - h[1] * v[3], -h[1] * v[2] + h[2] * v[1] + m * v[3],
                             h[0] * v[2] - h[2] * v[0] + m * v[4], -h[0] * v[1] + h[1] * v[0] + m * v[5]);
    }

    Matrix63 multiplyMatrix63(Matrix63 m) const
    {
        m.col(0) = this->timesSpatialVector(m.col(0));
        m.col(1) = this->timesSpatialVector(m.col(1));
        m.col(2) = this->timesSpatialVector(m.col(2));
        return m;
    }

    RigidBodyInertia& operator=(const RigidBodyInertia& other)
    {
        m = other.m;
        h = other.h;
        Ixx = other.Ixx;
        Iyx = other.Iyx;
        Iyy = other.Iyy;
        Izx = other.Izx;
        Izy = other.Izy;
        Izz = other.Izz;
        return *this;
    }

    double m; 
    Vector3d h; 
    double Ixx; 
    double Iyx; 
    double Iyy; 
    double Izx; 
    double Izy; 
    double Izz; 
};

inline RigidBodyInertia operator+(RigidBodyInertia rbi_in, const RigidBodyInertia& rbi)
{
    rbi_in += rbi;
    return rbi_in;
}

inline SpatialVector operator*(const RigidBodyInertia& I, const SpatialVector& v)
{
    return I.timesSpatialVector(v);
}

inline Matrix63 operator*(const RigidBodyInertia& I, const Matrix63& m)
{
    return I.multiplyMatrix63(m);
}

static inline RigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d& com, const Matrix3d& inertia_C)
{
    RigidBodyInertia result;

    result.m = mass;
    result.h = com * mass;
    Matrix3d I = inertia_C + toTildeForm(com) * toTildeForm(com).transpose() * mass;
    result.Ixx = I(0, 0);
    result.Iyx = I(1, 0);
    result.Iyy = I(1, 1);
    result.Izx = I(2, 0);
    result.Izy = I(2, 1);
    result.Izz = I(2, 2);

    //            RigidBodyInertia B(mass,com*mass,inertia_C + toTildeForm(com) * toTildeForm(com).transpose() * mass);
    return RigidBodyInertia(mass, com * mass, inertia_C + toTildeForm(com) * toTildeForm(com).transpose() * mass);
}
typedef std::vector<RigidBodyInertia, Eigen::aligned_allocator<RigidBodyInertia>> RigidBodyInertiaV;
}  // namespace Math
}  // namespace RobotDynamics

#endif  //__SPATIAL_INERTIA_HPP__