ForceVector.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_FORCE_VECTOR_HPP__
#define __RDL_FORCE_VECTOR_HPP__
 
#include "rdl_dynamics/rdl_eigenmath.hpp"
 
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[0], f[1], f[2], f[3], f[4], f[5];
    }
 
    EIGEN_STRONG_INLINE double& mx()
    {
        return this->operator[](0);
    }
 
    EIGEN_STRONG_INLINE double& my()
    {
        return this->operator[](1);
    }
 
    EIGEN_STRONG_INLINE double& mz()
    {
        return this->operator[](2);
    }
 
    EIGEN_STRONG_INLINE double mx() const
    {
        return this->operator[](0);
    }
 
    EIGEN_STRONG_INLINE double my() const
    {
        return this->operator[](1);
    }
 
    EIGEN_STRONG_INLINE double mz() const
    {
        return this->operator[](2);
    }
 
    EIGEN_STRONG_INLINE double& fx()
    {
        return this->operator[](3);
    }
 
    EIGEN_STRONG_INLINE double& fy()
    {
        return this->operator[](4);
    }
 
    EIGEN_STRONG_INLINE double& fz()
    {
        return this->operator[](5);
    }
 
    EIGEN_STRONG_INLINE double fx() const
    {
        return this->operator[](3);
    }
 
    EIGEN_STRONG_INLINE double fy() const
    {
        return this->operator[](4);
    }
 
    EIGEN_STRONG_INLINE double fz() const
    {
        return this->operator[](5);
    }
 
    void transform(const SpatialTransform& X)
    {
        this->setAngularPart(X.E * (this->getAngularPart() - X.r.cross(this->getLinearPart())));
        this->setLinearPart(X.E * this->getLinearPart());
    }
 
    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