RigidBodyAcceleration.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)
**********************************************************************************************************************/

#pragma once

namespace ct {
namespace rbd {
namespace tpl {

template <typename SCALAR>
class RigidBodyAcceleration
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef kindr::Acceleration<SCALAR, 3> LinearAcceleration;
    typedef kindr::AngularAcceleration<SCALAR, 3> AngularAcceleration;

    RigidBodyAcceleration() { setZero(); }
    RigidBodyAcceleration(LinearAcceleration& transAcceleration, AngularAcceleration& anglAcceleration)
        : anglAcceleration_(anglAcceleration), transAcceleration_(transAcceleration)
    {
    }

    RigidBodyAcceleration(const Eigen::Matrix<SCALAR, 6, 1>& in) { fromVector6d(in); }
    void fromVector6d(const Eigen::Matrix<SCALAR, 6, 1>& in)
    {
        anglAcceleration_ << in.template head<3>();
        transAcceleration_ << in.template tail<3>();
    }

    const Eigen::Matrix<SCALAR, 6, 1> getVector6d() const
    {
        Eigen::Matrix<SCALAR, 6, 1> vector6;
        vector6 << anglAcceleration_.toImplementation(), transAcceleration_.toImplementation();
        return vector6;
    }

    LinearAcceleration& getTranslationalAcceleration() { return transAcceleration_; }
    const LinearAcceleration& getTranslationalAcceleration() const { return transAcceleration_; }
    AngularAcceleration& getAngularAcceleration() { return anglAcceleration_; }
    const AngularAcceleration& getAngularAcceleration() const { return anglAcceleration_; }
    Eigen::Matrix<SCALAR, 6, 1> accelerations()
    {
        Eigen::Matrix<SCALAR, 6, 1> out;
        out << anglAcceleration_.toImplementation(), transAcceleration_.toImplementation();
        return out;
    }

    void setTranslatoryAcceleration(LinearAcceleration& transAcceleration) { transAcceleration_ = transAcceleration; }
    void setAngularAcceleration(AngularAcceleration& anglAcceleration) { anglAcceleration_ = anglAcceleration; }
    void setZero()
    {
        // cannot use .setZero() from members here due to codegen
        anglAcceleration_ = AngularAcceleration(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0));
        transAcceleration_ = LinearAcceleration(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0));
    }

    static RigidBodyAcceleration Zero()
    {
        RigidBodyAcceleration state;
        state.setZero();
        return state;
    }

protected:
    AngularAcceleration anglAcceleration_;
    LinearAcceleration transAcceleration_;
};

}  // namespace tpl

// convenience typedef (required)
typedef tpl::RigidBodyAcceleration<double> RigidBodyAcceleration;

}  // namespace rbd
}  // namespace ct