SpatialMotion.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_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;
    }
 
    void setIncludingFrame(ReferenceFramePtr referenceFrame, Vector3d angularPart, Vector3d linearPart)
    {
        this->SpatialVector::set(angularPart, linearPart);
        this->referenceFrame = referenceFrame;
    }
 
    SpatialMotion& operator=(const SpatialMotion& v)
    {
        SpatialVector::operator=(v);
        this->referenceFrame = v.getReferenceFrame();
        this->bodyFrame = v.getBodyFrame();
        this->baseFrame = v.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__