dc-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_motor_model_hpp_
#define _se3_motor_model_hpp_

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


namespace se3
{
  template<typename Scalar_> 
  class ActuatorDCMotorData : ActuatorDataBase<ActuatorDCMotorData<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_ROTOR_RESISTOR,
  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
      };
    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 rotorResistor(Scalar_ c)
    { c_[P_ROTOR_RESISTOR] = 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 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 aCurrant/c_[P_NOMINAL_CURRENT]*sqrt((c_[P_MAX_PERM_WINDING_T]- 25)/(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]- 25) *
               (c_[P_THONE_RESISTOR] + c_[P_THTWO_RESISTOR])/(c_[P_THONE_RESISTOR]));
    }
    
    Observations_t & h() { return h_;}
    
  protected:
    void updateParameters()
    {
      // c_1 = torque_cst / (rotorInertia * rotorResistor)
      c_[0] = c_[P_TORQUE_CST] /(c_[P_ROTOR_INERTIA] * c_[P_ROTOR_RESISTOR]);
      // c_2 = - torque_cst * back_emf_cst  / (rotorInertia * rotorResistor)
      //        - speedTorqueGrad^{-1} / rotorInertia
      c_[1] = -c_[P_TORQUE_CST]*c_[P_BACK_EMF] /(c_[P_ROTOR_INERTIA] * c_[P_ROTOR_RESISTOR])
  - c_[P_SPEED_TORQUE_GRD]/c_[P_ROTOR_INERTIA];
      // c_3 = -1/rotorInertia
      c_[2] = -1/c_[P_ROTOR_INERTIA];
    }

    Observations_t h_;
    
    Parameters_t c_;
    
    S_t S_;
    
  };

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

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

    void calc(typename ActuatorDCMotorData<Scalar_>::dX_t & dstate,
        typename ActuatorDCMotorData<Scalar_>::X_t & state,
        typename ActuatorDCMotorData<Scalar_>::U_t & control,
        Force & fext,
        ActuatorDCMotorData<Scalar_> &data)
    {
      // Update dstate
      dstate[0] = state[1];
      dstate[1] = data.c()[0] * control[0] + data.c()[1] * state[0] +
  data.c()[2] *data.S().dot(fext.toVector());
      
      // 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(ActuatorDCMotorData<Scalar_> &data, Force &aForce) const
    {
      aForce.linear(Vector3Scalar::Zero(3,1));
      aForce.angular(Vector3Scalar(0.0,0.0,data.h()[1]));
    }

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

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

  template <typename Scalar_>
  struct amb_traits<ActuatorDCMotorModel<Scalar_> >
  {
    typedef  ActuatorDCMotorModel<Scalar_> ActuatorModelDerived;
    typedef  ActuatorDCMotorData<Scalar_> ActuatorDataDerived;
  };


} // end of namespace se3

#endif /* _se3_motor_model_ */