SpatialForceVector.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#ifndef INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_
#define INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_

namespace ct {
namespace rbd {

template <typename SCALAR = double>
class SpatialForceVector : public Eigen::Matrix<SCALAR, 6, 1>
{
public:
    typedef Eigen::Matrix<SCALAR, 6, 1> spatial_force_vector_t;  
    typedef Eigen::VectorBlock<spatial_force_vector_t, 3>
        ForceTorqueBlock;  
    typedef Eigen::VectorBlock<const spatial_force_vector_t, 3>
        ForceTorqueBlockConst;  

    SpatialForceVector() {}
    SpatialForceVector(const spatial_force_vector_t& vector6) : spatial_force_vector_t(vector6) {}
    spatial_force_vector_t& toImplementation() { return *this; }
    const spatial_force_vector_t& toImplementation() const { return *this; }
    SpatialForceVector& setZero()
    {
        // for auto diff compatability
        SCALAR zero(0.0);
        this->setConstant(zero);
        return *this;
    }

    inline bool operator==(const spatial_force_vector_t& rhs) const { return Base::operator==(rhs); }
    inline bool operator!=(const spatial_force_vector_t& rhs) const { return Base::operator!=(rhs); }
    inline spatial_force_vector_t operator+(const spatial_force_vector_t& rhs) const
    {
        return spatial_force_vector_t(Base::operator+(rhs));
    }
    inline spatial_force_vector_t operator-(const spatial_force_vector_t& rhs) const
    {
        return spatial_force_vector_t(Base::operator-(rhs));
    }

    inline spatial_force_vector_t operator*(const SCALAR& scalar) const
    {
        return spatial_force_vector_t(Base::operator*(scalar));
    }

    inline spatial_force_vector_t operator/(const SCALAR& scalar) const
    {
        return spatial_force_vector_t(Base::operator/(scalar));
    }

    ForceTorqueBlock torque() { return this->template head<3>(); }
    const ForceTorqueBlockConst torque() const { return this->template head<3>(); }
    ForceTorqueBlock force() { return this->template tail<3>(); }
    const ForceTorqueBlockConst force() const { return this->template tail<3>(); }
private:
    typedef Eigen::Matrix<SCALAR, 6, 1> Base;
};
}
}


#endif /* INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_ */