SpatialMomentum.hpp

Go to the documentation of this file.

// Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
// RDL - Robot Dynamics Library
// 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(inertia, vector), FrameObject(inertia.getReferenceFrame())
    {
        inertia.getReferenceFrame()->checkReferenceFramesMatch(vector.getReferenceFrame());
    }
 
    SpatialMomentum(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
      : Momentum(inertia, vector), FrameObject(referenceFrame)
    {
    }
 
    SpatialMomentum& operator=(const SpatialMomentum& m)
    {
        this->referenceFrame = m.getReferenceFrame();
        SpatialVector::operator=(m);
        return *this;
    }
 
    void setIncludingFrame(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
    {
        this->referenceFrame = referenceFrame;
        SpatialVector::operator=(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)
{
    return SpatialMomentum(inertia, vector);
}
typedef std::vector<SpatialMomentum, Eigen::aligned_allocator<SpatialMomentum>> SpatialMomentumV;
}  // namespace Math
}  // namespace RobotDynamics
 
#endif  //__RDL_SPATIAL_MOMENTUM_HP__