SpatialForce.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_FORCE_HPP__
#define __RDL_SPATIAL_FORCE_HPP__
 
#include "rdl_dynamics/ForceVector.hpp"
#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/FrameVector.hpp"
 
namespace RobotDynamics
{
namespace Math
{
class SpatialForce : public ForceVector, public FrameObject
{
  public:
    SpatialForce() : ForceVector(), FrameObject(nullptr)
    {
    }
 
    explicit SpatialForce(ReferenceFramePtr referenceFrame) : ForceVector(), FrameObject(referenceFrame)
    {
    }
 
    SpatialForce(ReferenceFramePtr referenceFrame, const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
      : ForceVector(mx, my, mz, fx, fy, fz), FrameObject(referenceFrame)
    {
    }
 
    SpatialForce(ReferenceFramePtr referenceFrame, const Vector3d& m, const Vector3d f)
      : ForceVector(m.x(), m.y(), m.z(), f.x(), f.y(), f.z()), FrameObject(referenceFrame)
    {
    }
 
    SpatialForce& operator=(const SpatialForce& f)
    {
        this->referenceFrame = f.getReferenceFrame();
        SpatialVector::operator=(f);
        return *this;
    }
 
    Math::TransformableGeometricObject* getTransformableGeometricObject()
    {
        return this;
    }
 
    SpatialForce changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
    {
        SpatialForce ret = *this;
        ret.changeFrame(referenceFrame);
        return ret;
    }
 
    FrameVector getFramedLinearPart() const
    {
        return FrameVector(this->referenceFrame, this->getLinearPart());
    }
 
    FrameVector getFramedAngularPart() const
    {
        return FrameVector(this->referenceFrame, this->getAngularPart());
    }
 
    SpatialForce(const SpatialForce& spatialForce) : ForceVector(spatialForce), FrameObject(spatialForce.referenceFrame)
    {
    }
 
    SpatialForce(ReferenceFramePtr referenceFrame, const SpatialVector& spatialVector) : ForceVector(spatialVector), FrameObject(referenceFrame)
    {
    }
 
    inline ForceVector toForceVector() const
    {
        return ForceVector(this->mx(), this->my(), this->mz(), this->fx(), this->fy(), this->fz());
    }
 
    void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
    {
        this->set(v);
        this->referenceFrame = referenceFrame;
    }
 
    void setIncludingFrame(ReferenceFramePtr referenceFrame, double wx, double wy, double wz, double vx, double vy, double vz)
    {
        this->SpatialVector::set(wx, wy, wz, vx, vy, vz);
        this->referenceFrame = referenceFrame;
    }
 
    void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& m, const Vector3d& f)
    {
        this->SpatialVector::set(m.x(), m.y(), m.z(), f.x(), f.y(), f.z());
        this->referenceFrame = referenceFrame;
    }
 
    inline SpatialForce operator*=(double scale)
    {
        mx() *= scale;
        my() *= scale;
        mz() *= scale;
 
        fx() *= scale;
        fy() *= scale;
        fz() *= scale;
 
        return *this;
    }
 
    inline SpatialForce operator+=(const SpatialForce& f)
    {
        this->checkReferenceFramesMatch(&f);
 
        this->mx() += f.mx();
        this->my() += f.my();
        this->mz() += f.mz();
 
        this->fx() += f.fx();
        this->fy() += f.fy();
        this->fz() += f.fz();
 
        return *this;
    }
 
    inline SpatialForce operator-=(const SpatialForce& f)
    {
        this->checkReferenceFramesMatch(&f);
 
        this->mx() -= f.mx();
        this->my() -= f.my();
        this->mz() -= f.mz();
 
        this->fx() -= f.fx();
        this->fy() -= f.fy();
        this->fz() -= f.fz();
 
        return *this;
    }
};
 
inline SpatialForce operator+(SpatialForce f1, const SpatialForce& f2)
{
    return f1 += f2;
}
 
inline SpatialForce operator-(SpatialForce f1, const SpatialForce& f2)
{
    return f1 -= f2;
}
 
inline SpatialForce operator*(SpatialForce f1, double scale)
{
    return f1 *= scale;
}
typedef std::vector<SpatialForce, Eigen::aligned_allocator<SpatialForce>> SpatialForceV;
}  // namespace Math
}  // namespace RobotDynamics
 
#endif  // ifndef __RDL_SPATIAL_FORCE_HPP__