SpatialForce.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_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)
    {
    }

    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__