FixBaseRobotState.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 FixBaseRobotState
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using RigidBodyPose_t = tpl::RigidBodyPose<SCALAR>;
    using JointState_t = JointState<NJOINTS, SCALAR>;
    using RBDState_t = RBDState<NJOINTS, SCALAR>;

    static const size_t NDOF = NJOINTS;
    static const size_t NSTATE = 2 * NJOINTS + ACT_STATE_DIM;

    using actuator_state_vector_t = ct::core::StateVector<ACT_STATE_DIM, SCALAR>;
    using state_vector_t = ct::core::StateVector<NSTATE, SCALAR>;

    FixBaseRobotState() : joints_(), act_state_(actuator_state_vector_t::Zero()) {}
    FixBaseRobotState(const JointState_t& joints,
        const actuator_state_vector_t& actState = actuator_state_vector_t::Zero())
        : joints_(joints), act_state_(actState)
    {
    }

    FixBaseRobotState(const state_vector_t& robotState) : joints_(), act_state_() { fromStateVector(robotState); }
    FixBaseRobotState(const FixBaseRobotState& other) : joints_(other.joints_), act_state_(other.act_state_) {}
    ~FixBaseRobotState() {}
    static const JointState_t jointStateFromVector(const state_vector_t& robotState)
    {
        JointState_t jointState;
        jointState.getPositions() = robotState.template head<NJOINTS>();
        jointState.getVelocities() = robotState.template segment<NJOINTS>(NJOINTS);
        return jointState;
    }

    static const RBDState_t rbdStateFromVector(const state_vector_t& robotState,
        const RigidBodyPose_t& basePose = RigidBodyPose_t())
    {
        RBDState_t rbdState;
        rbdState.setZero();
        rbdState.basePose() = basePose;
        rbdState.joints() = jointStateFromVector(robotState);
        return rbdState;
    }

    static const actuator_state_vector_t actuatorStateFromVector(const state_vector_t& robotState)
    {
        actuator_state_vector_t actState = robotState.template tail<ACT_STATE_DIM>();
        return actState;
    }

    static const state_vector_t toFullState(const ct::core::StateVector<2 * NJOINTS, SCALAR>& jointState,
        const actuator_state_vector_t& x_act = actuator_state_vector_t::Zero())
    {
        state_vector_t robotState;
        robotState.template head<2 * NJOINTS>() = jointState.template head<2 * NJOINTS>();
        robotState.template tail<ACT_STATE_DIM>() = x_act;
        return robotState;
    }

    actuator_state_vector_t& actuatorState() { return act_state_; }
    const actuator_state_vector_t& actuatorState() const { return act_state_; }
    SCALAR& actuatorState(size_t i) { return act_state_(i); }
    const SCALAR& actuatorState(size_t i) const { return act_state_(i); }
    JointState_t& joints() { return joints_; }
    const JointState_t& joints() const { return joints_; }
    void setZero()
    {
        joints_.setZero();
        act_state_.setZero();
    }

    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type fromStateVector(const state_vector_t& robotState)
    {
        joints_.getPositions() = robotState.template head<NJOINTS>();
        joints_.getVelocities() = robotState.template segment<NJOINTS>(NJOINTS);
    }

    template <typename T = void>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type fromStateVector(const state_vector_t& robotState)
    {
        joints_.getPositions() = robotState.template head<NJOINTS>();
        joints_.getVelocities() = robotState.template segment<NJOINTS>(NJOINTS);
        act_state_ = robotState.template tail<ACT_STATE_DIM>();
    }

    template <typename T = state_vector_t>
    typename std::enable_if<(ACT_STATE_DIM == 0), T>::type toStateVector() const
    {
        state_vector_t robotState;
        robotState.template head<NJOINTS>() = joints_.getPositions();
        robotState.template segment<NJOINTS>(NJOINTS) = joints_.getVelocities();
        return robotState;
    }
    template <typename T = state_vector_t>
    typename std::enable_if<(ACT_STATE_DIM > 0), T>::type toStateVector() const
    {
        state_vector_t robotState;
        robotState.template head<NJOINTS>() = joints_.getPositions();
        robotState.template segment<NJOINTS>(NJOINTS) = joints_.getVelocities();
        robotState.template tail<ACT_STATE_DIM>() = act_state_;
        return robotState;
    }

    RBDState_t toRBDState(const RigidBodyPose_t& basePose = RigidBodyPose_t())
    {
        RBDState_t rbdState;
        rbdState.setZero();
        rbdState.basePose() = basePose;
        rbdState.joints() = joints_;
        return rbdState;
    }

protected:
    JointState_t joints_;
    actuator_state_vector_t act_state_;
};

}  // namespace rbd
}  // namespace ct