SEADynamicsFirstOrder.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 "ActuatorDynamics.h"

namespace ct {
namespace rbd {

template <size_t NJOINTS, typename SCALAR = double>
class SEADynamicsFirstOrder : public ActuatorDynamics<NJOINTS, NJOINTS, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ActuatorDynamics<NJOINTS, NJOINTS, SCALAR> BASE;

    SEADynamicsFirstOrder(double k_spring, double gear_ratio);

    virtual ~SEADynamicsFirstOrder();

    virtual SEADynamicsFirstOrder<NJOINTS, SCALAR>* clone() const override;

    virtual void computeActuatorDynamics(const JointState<NJOINTS, SCALAR>& robotJointState,
        const ct::core::StateVector<NJOINTS, SCALAR>& state,
        const SCALAR& t,
        const ct::core::ControlVector<NJOINTS, SCALAR>& control,
        ct::core::StateVector<NJOINTS, SCALAR>& derivative) override;

    virtual core::ControlVector<NJOINTS, SCALAR> computeControlOutput(
        const JointState<NJOINTS, SCALAR>& robotJointState,
        const typename BASE::act_state_vector_t& actState) override;

    virtual ct::core::StateVector<NJOINTS, SCALAR> computeStateFromOutput(
        const JointState<NJOINTS, SCALAR>& refRobotJointState,
        const core::ControlVector<NJOINTS, SCALAR>& refControl) override;

private:
    SCALAR k_;  
    SCALAR r_;  
};


}  // namespace rbd
}  // namespace ct