dc-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_non_linear_motor_model_hpp_
#define _se3_non_linear_motor_model_hpp_

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


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

    const Observations_t & h() const { return h_;}
    
    const Parameters_t & c() const { return c_;}
    
    const S_t & S() const { return S_;}

    enum InternalParameters
      {
  P_ROTOR_INERTIA=6,
  P_ROTOR_RESISTOR,
  P_TORQUE_CST,
  P_SPEED_TORQUE_GRD,
  P_BACK_EMF,
  P_TERMINAL_INDUCTANCE
      };
    void rotorInertia(Scalar_ c)
    { c_[P_ROTOR_INERTIA] = c; updateFirstParameters();}

    void rotorResistor(Scalar_ c)
    { c_[P_ROTOR_RESISTOR] = c; updateFirstParameters();}
    
    void torqueConst(Scalar_ c)
    { c_[P_TORQUE_CST] = c; updateFirstParameters();}
    
    void speedTorqueGrad(Scalar_ c) 
    {c_[P_SPEED_TORQUE_GRD] = c; updateFirstParameters();}
    
    void backEMF(Scalar_ c) 
    {c_[P_BACK_EMF] = c;updateFirstParameters();}

    void terminalInductance(Scalar_ c)
    {c_[P_TERMINAL_INDUCTANCE] = c; updateFirstParameters();}
    

    void setS(S_t &S)
    {S_ = S;}

    Observations_t & h() { return h_;}
    
  protected:
    void updateFirstParameters()
    {
      // c_1 = torque_cst 
      c_[0] = c_[P_TORQUE_CST]/c_[P_ROTOR_INERTIA];
      // c_2 = - speedTorqueGrad/rotorInertia
      c_[1] = -c_[P_SPEED_TORQUE_GRD]/c_[P_ROTOR_INERTIA];
      // c_3 = -1/rotorInertia
      c_[2] = -1/c_[P_ROTOR_INERTIA];
      // c_4 = rotorResistor/terminalInductance
      c_[3] = -c_[P_ROTOR_RESISTOR]/c_[P_TERMINAL_INDUCTANCE];
      // c_5 = 1/terminalInductance
      c_[4] = 1/c_[P_TERMINAL_INDUCTANCE];
      // c_6 = - backEMF/terminalInductance
      c_[5] = - c_[P_BACK_EMF]/c_[P_TERMINAL_INDUCTANCE];
    }

    Observations_t h_;
    
    Parameters_t c_;
    
    S_t S_;
    
  };

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

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

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

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

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

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

  template <typename Scalar_>
  struct amb_traits<ActuatorDCNonLinearMotorModel<Scalar_> >
  {
    typedef  ActuatorDCNonLinearMotorModel<Scalar_> ActuatorModelDerived;
    typedef  ActuatorDCNonLinearMotorData<Scalar_> ActuatorDataDerived;
  };


} // end of namespace se3

#endif /* _se3_non_linear_motor_model_ */