SpatialMomentum.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_MOMENTUM_HP__
#define __RDL_SPATIAL_MOMENTUM_HP__

#include "rdl_dynamics/SpatialRigidBodyInertia.hpp"
#include "rdl_dynamics/Momentum.hpp"
#include "rdl_dynamics/SpatialMotion.hpp"

namespace RobotDynamics
{
namespace Math
{
class SpatialMomentum : public Momentum, public FrameObject
{
  public:
    SpatialMomentum() : Momentum(), FrameObject(nullptr)
    {
    }

    SpatialMomentum(ReferenceFramePtr referenceFrame, const double kx, const double ky, const double kz, const double lx, const double ly, const double lz)
      : Momentum(kx, ky, kz, lx, ly, lz), FrameObject(referenceFrame)
    {
    }

    SpatialMomentum(ReferenceFramePtr referenceFrame, const Vector3d& k, const Vector3d l)
      : Momentum(k.x(), k.y(), k.z(), l.x(), l.y(), l.z()), FrameObject(referenceFrame)
    {
    }

    SpatialMomentum(const SpatialMomentum& SpatialMomentum) : Momentum(SpatialMomentum), FrameObject(SpatialMomentum.referenceFrame)
    {
    }

    SpatialMomentum(ReferenceFramePtr referenceFrame, const ForceVector& forceVector) : Momentum(forceVector), FrameObject(referenceFrame)
    {
    }

    explicit SpatialMomentum(ReferenceFramePtr referenceFrame) : Momentum(), FrameObject(referenceFrame)
    {
    }

    SpatialMomentum(const SpatialInertia& inertia, const SpatialMotion& vector) : Momentum(), FrameObject(inertia.getReferenceFrame())
    {
        set(inertia, vector);
    }

    void set(const SpatialInertia& inertia, const SpatialMotion& vector)
    {
        inertia.getReferenceFrame()->checkReferenceFramesMatch(vector.getReferenceFrame());
        this->referenceFrame = inertia.getReferenceFrame();
        computeMomentum(inertia, vector);
    }

    void setIncludingFrame(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
    {
        this->referenceFrame = referenceFrame;
        computeMomentum(inertia, vector);
    }

    void setIncludingFrame(ReferenceFramePtr referenceFrame, const ForceVector& f)
    {
        this->referenceFrame = referenceFrame;
        ForceVector::set(f);
    }

    inline double operator*(const SpatialMotion& vector)
    {
        this->checkReferenceFramesMatch(&vector);
        return Momentum::operator*(vector);
    }

  protected:
    TransformableGeometricObject* getTransformableGeometricObject()
    {
        return this;
    }
};

inline SpatialMomentum operator*(const SpatialInertia& inertia, const SpatialMotion& vector)
{
    inertia.getReferenceFrame()->checkReferenceFramesMatch(vector.getReferenceFrame());
    SpatialMomentum m(inertia, vector);

    return m;
}
typedef std::vector<SpatialMomentum, Eigen::aligned_allocator<SpatialMomentum>> SpatialMomentumV;
}  // namespace Math
}  // namespace RobotDynamics

#endif  //__RDL_SPATIAL_MOMENTUM_HP__