ForceVector.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_FORCE_VECTOR_HPP__
#define __RDL_FORCE_VECTOR_HPP__

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

namespace RobotDynamics
{
namespace Math
{
class ForceVector : public SpatialVector, public TransformableGeometricObject
{
  public:
    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    ForceVector(const Eigen::MatrixBase<OtherDerived>& other) : SpatialVector(other)
    {
    }

    template <typename OtherDerived>
    ForceVector& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        SpatialVector::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE ForceVector() : SpatialVector(0., 0., 0., 0., 0., 0.)
    {
    }

    EIGEN_STRONG_INLINE SpatialVector toSpatialVector() const
    {
        return *this;
    }

    ForceVector(const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
    {
        Base::_check_template_params();

        (*this) << mx, my, mz, fx, fy, fz;
    }

    EIGEN_STRONG_INLINE void set(const ForceVector& f)
    {
        (*this) << f.data()[0], f.data()[1], f.data()[2], f.data()[3], f.data()[4], f.data()[5];
    }

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

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

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

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

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

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

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

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

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

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

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

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

    void transform(const SpatialTransform& X)
    {
        Vector3d En_rxf = X.E * (this->getAngularPart() - X.r.cross(this->getLinearPart()));

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

    ForceVector transform_copy(const SpatialTransform& X) const
    {
        ForceVector f = *this;
        f.transform(X);
        return f;
    }

    inline ForceVector operator+=(const ForceVector& v)
    {
        (*this) << (this->mx() += v.mx()), (this->my() += v.my()), (this->mz() += v.mz()), (this->fx() += v.fx()), (this->fy() += v.fy()), (this->fz() += v.fz());
        return *this;
    }
};

inline ForceVector operator*(const SpatialTransform& X, ForceVector f)
{
    f.transform(X);
    return f;
}
typedef std::vector<ForceVector, Eigen::aligned_allocator<ForceVector>> ForceVectorV;
}  // namespace Math
}  // namespace RobotDynamics

#endif