MotionVector.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_MOTION_VECTOR_HPP__
#define __RDL_MOTION_VECTOR_HPP__
 
#include "rdl_dynamics/ForceVector.hpp"
#include "rdl_dynamics/rdl_eigenmath.hpp"
 
namespace RobotDynamics
{
namespace Math
{
class MotionVector : public SpatialVector, public TransformableGeometricObject
{
  public:
    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    MotionVector(const Eigen::MatrixBase<OtherDerived>& other) : SpatialVector(other)
    {
    }
 
    MotionVector(const MotionVector& v) : SpatialVector(v)
    {
    }
 
    MotionVector& operator=(const MotionVector& other)
    {
        SpatialVector::operator=(other);
        return *this;
    }
 
    EIGEN_STRONG_INLINE MotionVector() : SpatialVector(0., 0., 0., 0., 0., 0.)
    {
    }
 
    MotionVector(const double v0, const double v1, const double v2, const double v3, const double v4, const double v5)
    {
        Base::_check_template_params();
 
        (*this) << v0, v1, v2, v3, v4, v5;
    }
 
    EIGEN_STRONG_INLINE SpatialVector toSpatialVector() const
    {
        return *this;
    }
 
    EIGEN_STRONG_INLINE void set(const MotionVector& v)
    {
        (*this) << v[0], v[1], v[2], v[3], v[4], v[5];
    }
 
    EIGEN_STRONG_INLINE double& wx()
    {
        return this->operator[](0);
    }
 
    EIGEN_STRONG_INLINE double& wy()
    {
        return this->operator[](1);
    }
 
    EIGEN_STRONG_INLINE double& wz()
    {
        return this->operator[](2);
    }
 
    EIGEN_STRONG_INLINE double wx() const
    {
        return this->operator[](0);
    }
 
    EIGEN_STRONG_INLINE double wy() const
    {
        return this->operator[](1);
    }
 
    EIGEN_STRONG_INLINE double wz() const
    {
        return this->operator[](2);
    }
 
    EIGEN_STRONG_INLINE double& vx()
    {
        return this->operator[](3);
    }
 
    EIGEN_STRONG_INLINE double& vy()
    {
        return this->operator[](4);
    }
 
    EIGEN_STRONG_INLINE double& vz()
    {
        return this->operator[](5);
    }
 
    EIGEN_STRONG_INLINE double vx() const
    {
        return this->operator[](3);
    }
 
    EIGEN_STRONG_INLINE double vy() const
    {
        return this->operator[](4);
    }
 
    EIGEN_STRONG_INLINE double vz() const
    {
        return this->operator[](5);
    }
 
    inline void transform(const SpatialTransform& X)
    {
        this->setLinearPart(-X.E * X.r.cross(this->getAngularPart()) + X.E * this->getLinearPart());
        this->setAngularPart(X.E * this->getAngularPart());
    }
 
    MotionVector transform_copy(const SpatialTransform& X) const
    {
        MotionVector v = *this;
        v.transform(X);
        return v;
    }
 
    MotionVector cross(const MotionVector& v);
 
    ForceVector cross(const ForceVector& v);
 
    inline SpatialMatrix crossm()
    {
        return SpatialMatrix(0, -this->operator[](2), this->operator[](1), 0, 0, 0, this->operator[](2), 0, -this->operator[](0), 0, 0, 0, -this->operator[](1),
                             this->operator[](0), 0, 0, 0, 0, 0, -this->operator[](5), this->operator[](4), 0, -this->operator[](2), this->operator[](1),
                             this->operator[](5), 0, -this->operator[](3), this->operator[](2), 0, -this->operator[](0), -this->operator[](4), this->operator[](3), 0,
                             -this->operator[](1), this->operator[](0), 0);
    }
 
    inline SpatialMatrix crossf()
    {
        return SpatialMatrix(0, -this->operator[](2), this->operator[](1), 0, -this->operator[](5), this->operator[](4), this->operator[](2), 0, -this->operator[](0),
                             this->operator[](5), 0, -this->operator[](3), -this->operator[](1), this->operator[](0), 0, -this->operator[](4), this->operator[](3), 0, 0,
                             0, 0, 0, -this->operator[](2), this->operator[](1), 0, 0, 0, this->operator[](2), 0, -this->operator[](0), 0, 0, 0, -this->operator[](1),
                             this->operator[](0), 0);
    }
 
    inline MotionVector operator%=(const MotionVector& v)
    {
        return this->cross(v);
    }
 
    inline ForceVector operator%=(const ForceVector& v)
    {
        return this->cross(v);
    }
 
    inline MotionVector operator+=(const MotionVector& v)
    {
        (*this) << (this->wx() += v.wx()), (this->wy() += v.wy()), (this->wz() += v.wz()), (this->vx() += v.vx()), (this->vy() += v.vy()), (this->vz() += v.vz());
        return *this;
    }
};
 
inline MotionVector operator*(const SpatialTransform& X, MotionVector v)
{
    v.transform(X);
    return v;
}
 
inline MotionVector operator%(MotionVector v, const MotionVector& v2)
{
    return v.MotionVector::operator%=(v2);
}
 
inline ForceVector operator%(MotionVector v, const ForceVector& v2)
{
    return v.MotionVector::operator%=(v2);
}
typedef std::vector<MotionVector, Eigen::aligned_allocator<MotionVector>> MotionVectorV;
}  // namespace Math
}  // namespace RobotDynamics
 
#endif