FloatingBaseRobotState.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

#include "RBDState.h"

namespace ct {
namespace rbd {

template <size_t NJOINTS, size_t ACT_STATE_DIM = 0, typename SCALAR = double>
class FloatingBaseRobotState : public RBDState<NJOINTS, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base_t = RBDState<NJOINTS, SCALAR>;
    using RBDState_t = RBDState<NJOINTS, SCALAR>;

    using RigidBodyPose_t = tpl::RigidBodyPose<SCALAR>;
    using RigidBodyState_t = tpl::RigidBodyState<SCALAR>;
    using JointState_t = JointState<NJOINTS, SCALAR>;
    using STORAGE_TYPE = typename RigidBodyPose_t::STORAGE_TYPE;  

    using actuator_state_vector_t = ct::core::StateVector<ACT_STATE_DIM, SCALAR>;
    static const size_t NDOF = Base_t::NDOF;
    static const size_t NSTATE = Base_t::NSTATE + ACT_STATE_DIM;
    static const size_t NSTATE_QUAT = NSTATE + 1;

    using state_vector_quat_t = ct::core::StateVector<NSTATE_QUAT, SCALAR>;
    using state_vector_euler_t = ct::core::StateVector<NSTATE, SCALAR>;


    FloatingBaseRobotState(const STORAGE_TYPE storage = STORAGE_TYPE::EULER)
        : Base_t(storage), act_state_(actuator_state_vector_t::Zero())
    {
    }

    FloatingBaseRobotState(const RBDState_t& rbdState,
        const actuator_state_vector_t& act_state = actuator_state_vector_t::Zero())
        : Base_t(rbdState), act_state_(act_state)
    {
    }

    FloatingBaseRobotState(const RigidBodyState_t& baseState,
        const JointState_t& jointState,
        const actuator_state_vector_t& act_state = actuator_state_vector_t::Zero())
        : Base_t(baseState, jointState), act_state_(act_state)
    {
    }

    FloatingBaseRobotState(const FloatingBaseRobotState& other) : Base_t(other), act_state_(other.act_state_) {}
    ~FloatingBaseRobotState() {}
    RBDState_t& rbdState() { return *this; }
    const RBDState_t& rbdState() const { return *this; }
    actuator_state_vector_t& actuatorState() { return act_state_; }
    const actuator_state_vector_t& actuatorState() const { return act_state_; }
    template <typename T = state_vector_quat_t>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type toStateVectorQuaternion() const
    {
        // assemble overall state from RBDState and actuator state
        state_vector_quat_t state;
        state << Base_t::toStateVectorQuaternion(), act_state_;
        return state;
    }
    template <typename T = state_vector_quat_t>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type toStateVectorQuaternion() const
    {
        // transcribe RBDState only
        return Base_t::toStateVectorQuaternion();
    }

    template <typename T = state_vector_euler_t>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type toStateVectorEulerXyzUnique() const
    {
        // assemble overall state from RBDState and actuator state
        state_vector_euler_t state;
        state << Base_t::toStateVectorEulerXyzUnique(), act_state_;
        return state;
    }
    template <typename T = state_vector_euler_t>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type toStateVectorEulerXyzUnique() const
    {
        // transcribe RBDState only
        return Base_t::toStateVectorEulerXyzUnique();
    }

    template <typename T = state_vector_euler_t>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type toStateVectorEulerXyz() const
    {
        // the overall state consists of RBDState and actuator state
        state_vector_euler_t state;
        state << Base_t::toStateVectorEulerXyz(), act_state_;
        return state;
    }
    template <typename T = state_vector_euler_t>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type toStateVectorEulerXyz() const
    {
        // transcribe RBDState only
        return Base_t::toStateVectorEulerXyz();
    }

    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type fromStateVectorQuaternion(const state_vector_quat_t& state)
    {
        // transcribe the first part of the state in to the RBDState and the second part into the actuator state
        Eigen::VectorBlock<const Eigen::Matrix<SCALAR, NSTATE_QUAT, 1>, Base_t::NSTATE_QUAT> stateBase =
            state.template head<Base_t::NSTATE_QUAT>();
        Base_t::fromStateVectorQuaternion(stateBase);
        act_state_ = state.template tail<ACT_STATE_DIM>();
    }
    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type fromStateVectorQuaternion(const state_vector_quat_t& state)
    {
        // transcribe RBDState only
        Base_t::fromStateVectorQuaternion(state);
    }

    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type fromStateVectorEulerXyz(const state_vector_euler_t& state)
    {
        // transcribe the first part of the state in to the RBDState and the second part into the actuator state
        Eigen::VectorBlock<const Eigen::Matrix<SCALAR, NSTATE, 1>, Base_t::NSTATE> stateBase =
            state.template head<Base_t::NSTATE>();
        Base_t::fromStateVectorEulerXyz(stateBase);
        act_state_ = state.template tail<ACT_STATE_DIM>();
    }
    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type fromStateVectorEulerXyz(const state_vector_euler_t& state)
    {
        // transcribe RBDState only
        Base_t::state_vector_euler_t(state);
    }

    void fromStateVectorRaw(const state_vector_quat_t& state) { fromStateVectorQuaternion(state); }
    void fromStateVectorRaw(const state_vector_euler_t& state) { fromStateVectorEulerXyz(state); }
    void setZero()
    {
        act_state_.setZero();
        Base_t::setZero();
    }

protected:
    actuator_state_vector_t act_state_;

private:
    /*the following methods are not to be called from the Robot state. They can be accessed via rbdState(). */
    using Base_t::toCoordinatePositionUnique;
    using Base_t::toCoordinatePosition;
    using Base_t::toCoordinateVelocity;
};

}  // namespace rbd
}  // namespace ct