MotionVector.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_MOTION_VECTOR_HPP__
#define __RDL_MOTION_VECTOR_HPP__

#include "rdl_dynamics/ForceVector.hpp"
#include "rdl_dynamics/rdl_eigenmath.h"
#include "rdl_dynamics/SpatialAlgebraOperators.h"

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& 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.data()[0], v.data()[1], v.data()[2], v.data()[3], v.data()[4], v.data()[5];
    }

    EIGEN_STRONG_INLINE double& wx()
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double& wy()
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double& wz()
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double wx() const
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double wy() const
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double wz() const
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double& vx()
    {
        return this->data()[3];
    }

    EIGEN_STRONG_INLINE double& vy()
    {
        return this->data()[4];
    }

    EIGEN_STRONG_INLINE double& vz()
    {
        return this->data()[5];
    }

    EIGEN_STRONG_INLINE double vx() const
    {
        return this->data()[3];
    }

    EIGEN_STRONG_INLINE double vy() const
    {
        return this->data()[4];
    }

    EIGEN_STRONG_INLINE double vz() const
    {
        return this->data()[5];
    }

    inline void transform(const SpatialTransform& X)
    {
        Vector3d v_rxw(this->data()[3] - X.r[1] * this->data()[2] + X.r[2] * this->data()[1], this->data()[4] - X.r[2] * this->data()[0] + X.r[0] * this->data()[2],
                       this->data()[5] - X.r[0] * this->data()[1] + X.r[1] * this->data()[0]);

        *this << X.E(0, 0) * this->data()[0] + X.E(0, 1) * this->data()[1] + X.E(0, 2) * this->data()[2],
            X.E(1, 0) * this->data()[0] + X.E(1, 1) * this->data()[1] + X.E(1, 2) * this->data()[2],
            X.E(2, 0) * this->data()[0] + X.E(2, 1) * this->data()[1] + X.E(2, 2) * this->data()[2], X.E(0, 0) * v_rxw[0] + X.E(0, 1) * v_rxw[1] + X.E(0, 2) * v_rxw[2],
            X.E(1, 0) * v_rxw[0] + X.E(1, 1) * v_rxw[1] + X.E(1, 2) * v_rxw[2], X.E(2, 0) * v_rxw[0] + X.E(2, 1) * v_rxw[1] + X.E(2, 2) * v_rxw[2];
    }

    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->data()[2], this->data()[1], 0, 0, 0, this->data()[2], 0, -this->data()[0], 0, 0, 0, -this->data()[1], this->data()[0], 0, 0, 0, 0,
                             0, -this->data()[5], this->data()[4], 0, -this->data()[2], this->data()[1], this->data()[5], 0, -this->data()[3], this->data()[2], 0,
                             -this->data()[0], -this->data()[4], this->data()[3], 0, -this->data()[1], this->data()[0], 0);
    }

    inline SpatialMatrix crossf()
    {
        return SpatialMatrix(0, -this->data()[2], this->data()[1], 0, -this->data()[5], this->data()[4], this->data()[2], 0, -this->data()[0], this->data()[5], 0,
                             -this->data()[3], -this->data()[1], this->data()[0], 0, -this->data()[4], this->data()[3], 0, 0, 0, 0, 0, -this->data()[2], this->data()[1],
                             0, 0, 0, this->data()[2], 0, -this->data()[0], 0, 0, 0, -this->data()[1], this->data()[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