dc-temp-non-linear-motor-model.hpp

//
// Copyright (c) 2017 LAAS CNRS
//
// Author: Olivier Stasse
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef _se3_temp_non_linear_motor_model_hpp_
#define _se3_temp_non_linear_motor_model_hpp_

#include <iostream>
#include "pinocchio/macros.hpp"
#include "pinocchio/actuators/actuator-model.hpp"


namespace se3
{
  template<typename Scalar_> 
  class ActuatorDCTempNonLinearMotorData : ActuatorDataBase<ActuatorDCTempNonLinearMotorData<Scalar_> >
  {
  public:
    typedef Scalar_ Scalar_t;
    typedef typename Eigen::Matrix<Scalar_, 13,1 > Parameters_t;
    typedef typename Eigen::Matrix<Scalar_, 3,1> Observations_t;
    typedef typename Eigen::Matrix<Scalar_, 6,1> S_t;
    typedef typename Eigen::Matrix<Scalar_, 2,1> X_t;
    typedef typename Eigen::Matrix<Scalar_, 2,1> dX_t;
    typedef typename Eigen::Matrix<Scalar_, 1,1> U_t;

    enum InternalParameters
      {
  P_ROTOR_INERTIA=6,
  P_TORQUE_CST,
  P_SPEED_TORQUE_GRD,
  P_BACK_EMF,
  P_THONE_RESISTOR,
  P_THTWO_RESISTOR,
  P_TA_RESISTOR,
  P_MAX_PERM_WINDING_T,
  P_THERM_TIME_CST_WINDING,
  P_AMBIANT_TEMP
      };
    const Observations_t & h() const { return h_;}
    
    const Parameters_t & c() const { return c_;}
    
    const S_t & S() const { return S_;}

    void rotorInertia(Scalar_ c)
    { c_[P_ROTOR_INERTIA] = c; updateParameters();}

    
    void torqueConst(Scalar_ c)
    { c_[P_TORQUE_CST] = c; updateParameters();}
    
    void speedTorqueGrad(Scalar_ c) 
    {c_[P_SPEED_TORQUE_GRD] = c; updateParameters();}
    
    //void terminalInductance(Scalar_ &c)
    //    {c_[7] = c;}
    
    void backEMF(Scalar_ c) 
    {c_[P_BACK_EMF] = c;updateParameters();}

    void resistorOne(Scalar_ c)
    {c_[P_THONE_RESISTOR] = c;updateParameters();}
    
    void resistorTwo(Scalar_ c)
    {c_[P_THTWO_RESISTOR] = c;updateParameters();}

    void resistorTA(Scalar_ c)
    {c_[P_TA_RESISTOR] = c;updateParameters();}
      
    void maxPermissibleWindingTemp(Scalar_ c)
    {c_[P_MAX_PERM_WINDING_T] = c;updateParameters();}

    void thermTimeCstWinding(Scalar_ c)
    {c_[P_THERM_TIME_CST_WINDING] = c; updateParameters();}

    void thermAmbient(Scalar_ c)
    {c_[P_AMBIENT_TEMP] = c; updateParameters();}
    
    void setS(S_t &S)
    {S_ = S;}

    Scalar_ getMaxPermissibleOverloadForONTime(Scalar_ aDuration)
    {
      return sqrt(1/(1-exp(-aDuration/c_[P_THERM_TIME_CST_WINDING])));
    }

    Scalar_ getOverloadForCurrent(Scalar_ aCurrent, Scalar_ TS)
    {
      return aCurrent/c_[P_NOMINAL_CURRENT]*sqrt((c_[P_MAX_PERM_WINDING_T]- c_[P_AMBIENT_TEMP)/(c_[P_MAX_PERM_WINDING_T] - TS) *
             (c_[P_THONE_RESISTOR]/(c_[P_THONE_RESISTOR] + c_[P_THTWO_RESISTOR])));
    }

    Scalar_ getMaxONTimeForK(Scalar_ aK)
    {
      return c_[P_THERM_TIME_CST_WINDING] * log(aK*aK/(aK*aK-1.0));
    }

    Scalar_ getMaxCurrentForK(Scalar_ aK)
    {
      return aK * c_[P_NOMINAL_CURRENT]*sqrt((c_[P_MAX_PERM_WINDING_T] - TS)/(c_[P_MAX_PERM_WINDING_T]- c_[P_AMBIENT_TEMP]) *
               (c_[P_THONE_RESISTOR] + c_[P_THTWO_RESISTOR])/(c_[P_THONE_RESISTOR]));
    }
    
    Observations_t & h() { return h_;}
    
  protected:
    void updateParameters()
    {
      // c_1 = torque_cst 
      c_[0] = c_[P_TORQUE_CST];

      // c_2 = B_m R_{TA} (1+T_A \alpha_{Cu} )
      c_[1] = c_[P_ROTOR_INERTIA] * c_[P_TA_RESISTOR])*(1+ c_[P_AMBIENT_TEMP] * alpha_cu);
    
      // c_3 = -B_m R_{TA} \alpha_{Cu}
      c_[2] = -c_[P_ROTOR_INERTIA] * c_[P_TA_RESISTOR] * alpha_cu_;
      
      // c_4 = K_B + D_m R_{TA} + D_m R_{TA} \alpha_{Cu} T_{A}
      c_[3] = c_[P_BACK_EMF] + c_[P_SPEED_TORQUE_GRD]*c_[P_TA_RESISTOR] (1+alpha_cu_ * c_[P_AMBIENT_TEMP]);

      // c_5 = -D_m R_{TA} \alpha_{Cu}
      c_[4] = -c_[P_SPEED_TORQUE_GRD] * c_[P_TA_RESISTOR] * alpha_cu_;

      // c_6 = -frac{1}{B_m}
      c_[5] = -1/c_[P_ROTOR_INERTIA];

      // c_7 = (R_{th1} + R_{th2})
      c_[6] = c_[P_THONE_RESISTOR] + c_[P_THTWO_RESISTOR];

      // c_8 = -K_b
      c_[7] = -c_[P_BACK_EMF];

      c_[8] = c_[P_TA_RESISTOR] - alpha_cu_ * c_[P_AMBIENT_TEMP] * c_[P_TA_RESISTOR];

      // \f$ c_{10} = \alpha_{Cu} R_{TA} \f$
      c_[9] = alpha_cu_ *c_[P_TA_RESISTOR];

      // \f$ c_{11} = -\alpha_{Cu} (R_{th1} + R_{th2}) \f$
      c_[10] = alpha_cu_ * (c_[P_THONE_RESISTOR] + c_[P_THTWO_RESISTOR]);
      
    }

    Observations_t h_;
    
    Parameters_t c_;
    
    S_t S_;

    double alpha_cu_;
    
  };

  template< typename Scalar_> 
  class ActuatorDCTempNonLinearMotorModel : ActuatorModelBase<ActuatorDCTempNonLinearMotorModel<Scalar_> >
  {

    typedef Eigen::Matrix<Scalar_,3,1,0> Vector3Scalar;
    
  public:
    ActuatorDCTempNonLinearMotorModel() {}

    void calc(typename ActuatorDCTempNonLinearMotorData<Scalar_>::dX_t & dstate,
        typename ActuatorDCTempNonLinearMotorData<Scalar_>::X_t & state,
        typename ActuatorDCTempNonLinearMotorData<Scalar_>::U_t & control,
        Force & fext,
        ActuatorDCTempNonLinearMotorData<Scalar_> &data)
    {
      // Update dstate
      dstate[0] = state[1];
      dstate[1] = data.c()[0]/(data.c()[1] + data.c()[2]*state[2]) * control[0] +
  state[1]*(data.c()[3]+data.c()[4]*state[2])/(data.c()[1] + data.c()[2]*state[2])
  + data.c()[5] *data.S().dot(fext.toVector());
      dstate[2] = 
      
      // Update observation
      // Current
      data.h()[0] = control[0] /data.c()[P_ROTOR_RESISTOR]
  - data.c()[P_BACK_EMF] *state[1]/data.c()[P_ROTOR_RESISTOR];
      // Motor torque
      data.h()[1] = data.c()[P_TORQUE_CST] *data.h()[0];
      // Back emf potential 
      data.h()[2] = data.c()[P_BACK_EMF] * state[1];
    }
    

    void get_force(ActuatorDCTempNonLinearMotorData<Scalar_> &data, Force &aForce) const
    {
      aForce.linear(Vector3Scalar::Zero(3,1));
      aForce.angular(Vector3Scalar(0.0,0.0,data.h()[1]));
    }

    ActuatorDCTempNonLinearMotorData<Scalar_> createData() const
    { return ActuatorDCTempNonLinearMotorData<Scalar_>(); };
    
  protected:
  
    // Store actuator name.
    std::string name_;

  };
  
  template <typename Scalar_>
  struct adb_traits<ActuatorDCTempNonLinearMotorData<Scalar_> >
  {
    typedef ActuatorDCTempNonLinearMotorData<Scalar_> ActuatorDataDerived;
  };

  template <typename Scalar_>
  struct amb_traits<ActuatorDCTempNonLinearMotorModel<Scalar_> >
  {
    typedef  ActuatorDCTempNonLinearMotorModel<Scalar_> ActuatorModelDerived;
    typedef  ActuatorDCTempNonLinearMotorData<Scalar_> ActuatorDataDerived;
  };


} // end of namespace se3

#endif /* _se3_temp_non_linear_motor_model_ */