SpatialRigidBodyInertia.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_SPATIAL_RIGID_BODY_INERTIA_HPP__
#define __RDL_SPATIAL_RIGID_BODY_INERTIA_HPP__

#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/RigidBodyInertia.hpp"

namespace RobotDynamics
{
namespace Math
{
class SpatialInertia : public RigidBodyInertia, public FrameObject
{
  public:
    SpatialInertia() : RigidBodyInertia(), FrameObject(nullptr)
    {
    }

    explicit SpatialInertia(ReferenceFramePtr referenceFrame) : RigidBodyInertia(), FrameObject(referenceFrame)
    {
    }

    SpatialInertia(ReferenceFramePtr referenceFrame, double mass, const Vector3d& com_mass, const Matrix3d& inertia)
      : RigidBodyInertia(mass, com_mass, inertia), FrameObject(referenceFrame)
    {
    }

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

    SpatialInertia(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia) : RigidBodyInertia(inertia), FrameObject(referenceFrame)
    {
    }

    SpatialInertia(const SpatialInertia& inertia) : RigidBodyInertia(inertia), FrameObject(inertia.referenceFrame)
    {
    }

    TransformableGeometricObject* getTransformableGeometricObject()
    {
        return this;
    }

    RigidBodyInertia toRigidBodyInertia() const
    {
        return RigidBodyInertia(this->m, this->h, this->Ixx, this->Iyx, this->Iyy, this->Izx, this->Izy, this->Izz);
    }

    void operator+=(const SpatialInertia& spatialInertia)
    {
        checkReferenceFramesMatch(&spatialInertia);
        this->RigidBodyInertia::operator+=(spatialInertia);
    }

    RobotDynamics::Math::SpatialForce operator*(const RobotDynamics::Math::SpatialAcceleration& a)
    {
        referenceFrame->checkReferenceFramesMatch(a.getReferenceFrame());
        return RobotDynamics::Math::SpatialForce(referenceFrame, this->timesSpatialVector(a));  // this->RigidBodyInertia::operator*(*this, a));
    }
};

inline SpatialInertia operator+(SpatialInertia inertia_a, const SpatialInertia& inertia_b)
{
    inertia_a += inertia_b;
    return inertia_a;
}
typedef std::vector<SpatialInertia, Eigen::aligned_allocator<SpatialInertia>> SpatialInertiaV;
}  // namespace Math
}  // namespace RobotDynamics

#endif  //__SPATIAL_RIGID_BODY_INERTIA_HPP__