SpatialMotion.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_MOTION_HPP__
#define __RDL_SPATIAL_MOTION_HPP__

#include "rdl_dynamics/FrameObject.hpp"
#include "rdl_dynamics/FrameVector.hpp"
#include "rdl_dynamics/MotionVector.hpp"

namespace RobotDynamics
{
namespace Math
{
class SpatialMotion : public MotionVector, public FrameObject
{
  public:
    SpatialMotion() : MotionVector(), FrameObject(nullptr)
    {
        bodyFrame = nullptr;
        baseFrame = nullptr;
    }

    SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const double wx, const double wy, const double wz,
                  const double vx, const double vy, const double vz)
      : MotionVector(wx, wy, wz, vx, vy, vz), FrameObject(expressedInFrame)
    {
        this->bodyFrame = bodyFrame;
        this->baseFrame = baseFrame;
    }

    SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const Vector3d& w, const Vector3d v)
      : MotionVector(w.x(), w.y(), w.z(), v.x(), v.y(), v.z()), FrameObject(expressedInFrame)
    {
        this->bodyFrame = bodyFrame;
        this->baseFrame = baseFrame;
    }

    SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const SpatialVector& v)
      : MotionVector(v), FrameObject(expressedInFrame)
    {
        this->bodyFrame = bodyFrame;
        this->baseFrame = baseFrame;
    }

    SpatialMotion(const SpatialMotion& spatialMotion) : MotionVector(spatialMotion), FrameObject(spatialMotion.referenceFrame)
    {
        this->bodyFrame = spatialMotion.bodyFrame;
        this->baseFrame = spatialMotion.baseFrame;
    }

    FrameVector getFramedLinearPart() const
    {
        return FrameVector(this->referenceFrame, this->getLinearPart());
    }

    FrameVector getFramedAngularPart() const
    {
        return FrameVector(this->referenceFrame, this->getAngularPart());
    }

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

    SpatialMotion changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
    {
        SpatialMotion ret = *this;
        ret.changeFrame(referenceFrame);
        return ret;
    }

    inline ReferenceFramePtr getBodyFrame() const
    {
        return bodyFrame;
    }

    inline ReferenceFramePtr getBaseFrame() const
    {
        return baseFrame;
    }

    inline MotionVector toMotionVector() const
    {
        return *this;
    }

    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;
    }

    SpatialMotion& operator=(const SpatialMotion& other)
    {
        MotionVector::operator=(other);
        this->referenceFrame = other.getReferenceFrame();
        this->bodyFrame = other.getBodyFrame();
        this->baseFrame = other.getBaseFrame();
        return *this;
    }

    void operator+=(const SpatialMotion& v);

    void operator-=(const SpatialMotion& v);

    void operator%=(const SpatialMotion& v);

    void setBodyFrame(ReferenceFramePtr referenceFrame)
    {
        this->bodyFrame = referenceFrame;
    }

    void setBaseFrame(ReferenceFramePtr referenceFrame)
    {
        this->baseFrame = referenceFrame;
    }

  protected:
    ReferenceFramePtr bodyFrame;
    ReferenceFramePtr baseFrame;
};

EIGEN_STRONG_INLINE SpatialMotion operator+(SpatialMotion v1, const SpatialMotion& v2)
{
    v1 += v2;
    return v1;
}

EIGEN_STRONG_INLINE SpatialMotion operator-(SpatialMotion v1, const SpatialMotion& v2)
{
    v1 -= v2;
    return v1;
}

EIGEN_STRONG_INLINE SpatialMotion operator%(SpatialMotion v1, const SpatialMotion& v2)
{
    v1.SpatialMotion::operator%=(v2);
    return v1;
}
typedef std::vector<SpatialMotion, Eigen::aligned_allocator<SpatialMotion>> SpatialMotionV;
}  // namespace Math
}  // namespace RobotDynamics

#endif  //__RDL_SPATIAL_MOTION_HPP__