FixBaseSystemBase.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 <ct/rbd/state/RigidBodyPose.h>

#include "RBDSystem.h"

namespace ct {
namespace rbd {

template <class RBDDynamics, size_t STATE_D, size_t CONTROL_D>
class FixBaseSystemBase : public RBDSystem<RBDDynamics, false>,
                          public core::ControlledSystem<STATE_D, CONTROL_D, typename RBDDynamics::SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const size_t N_EE = RBDDynamics::N_EE;
    static const size_t NJOINTS = RBDDynamics::NJOINTS;

    using Dynamics = RBDDynamics;
    using SCALAR = typename RBDDynamics::SCALAR;
    using Base = core::ControlledSystem<STATE_D, CONTROL_D, SCALAR>;
    using RigidBodyPose_t = tpl::RigidBodyPose<SCALAR>;

    // typedefs state and controls
    using state_vector_t = ct::core::StateVector<STATE_D, SCALAR>;
    using control_vector_t = core::ControlVector<CONTROL_D, SCALAR>;
    using JointAcceleration_t = JointAcceleration<NJOINTS, SCALAR>;

    FixBaseSystemBase(const RigidBodyPose_t& basePose = RigidBodyPose_t())
        : Base(), basePose_(basePose), dynamics_(RBDDynamics())
    {
    }

    FixBaseSystemBase(const FixBaseSystemBase& arg) : Base(arg), basePose_(arg.basePose_), dynamics_(RBDDynamics()) {}
    virtual ~FixBaseSystemBase() = default;

    virtual RBDDynamics& dynamics() override { return dynamics_; }
    virtual const RBDDynamics& dynamics() const override { return dynamics_; }
    virtual void computeControlledDynamics(const ct::core::StateVector<STATE_D, SCALAR>& state,
        const SCALAR& t,
        const ct::core::ControlVector<CONTROL_D, SCALAR>& controlIn,
        ct::core::StateVector<STATE_D, SCALAR>& derivative) override = 0;

    virtual FixBaseSystemBase<RBDDynamics, STATE_D, CONTROL_D>* clone() const override = 0;

    ct::core::ControlVector<NJOINTS> computeIDTorques(const JointState<NJOINTS, SCALAR>& jState,
        const JointAcceleration_t& jAcc = JointAcceleration_t(Eigen::Matrix<SCALAR, NJOINTS, 1>::Zero()))
    {
        ct::core::ControlVector<NJOINTS> torque;
        dynamics_.FixBaseID(jState, jAcc, torque);
        return torque;
    }

protected:
    tpl::RigidBodyPose<SCALAR> basePose_;

    RBDDynamics dynamics_;
};

}  // namespace rbd
}  // namespace ct