dc-linear-first-order-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_dc_linear_first_order_motor_model_hpp_
#define _se3_dc_linear_first_order_motor_model_hpp_


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


namespace se3
{
  template<typename Scalar_> 
  class ActuatorDCFirstOrderMotorData : ActuatorDataBase<ActuatorDCFirstOrderMotorData<Scalar_> >
  {
  public:
    typedef Scalar_ Scalar_t;
    typedef typename Eigen::Matrix<Scalar_, 10,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;

    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_[3] = c; updateFirstThreeParameters();}

    void rotorResistor(Scalar_ c)
    { c_[4] = c; updateFirstThreeParameters();}
    
    void torqueConst(Scalar_ c)
    { c_[5] = c; updateFirstThreeParameters();}
    
    void speedTorqueGrad(Scalar_ c) 
    {c_[6] = 1/c; updateFirstThreeParameters();}
    
    //void terminalInductance(Scalar_ &c)
    //    {c_[7] = c;}
    
    void backEMF(Scalar_ c) 
    {c_[7] = c;}

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

    Observations_t & h() { return h_;}
    
  protected:
    void updateFirstThreeParameters()
    {
      // c_1 = torque_cst / (rotorInertia * rotorResistor)
      c_[0] = c_[5] /(c_[3] * c_[4]);
      // c_2 = - back_emf_cst  / (rotorInertia * rotorResistor)
      //        - speedTorqueGrad^{-1} / rotorInertia
      c_[1] = -c_[5]*c_[7] /(c_[5] + c_[3]*c_[6] ) ;
    }

    Observations_t h_;
    
    Parameters_t c_;
    
    S_t S_;
    
    X_t x_;
  };

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

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

    void calc(typename ActuatorDCFirstOrderMotorData<Scalar_>::dX_t & dstate,
        typename ActuatorDCFirstOrderMotorData<Scalar_>::X_t & state,
        typename ActuatorDCFirstOrderMotorData<Scalar_>::U_t & control,
        Force & fext,
        ActuatorDCFirstOrderMotorData<Scalar_> &data)
    {
      // Update dstate
      dstate[0] = 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()[4] - data.c()[7] *state[1]/data.c()[4];
      // Motor torque
      data.h()[1] = data.c()[5] *data.h()[0];
      // Back emf potential 
      data.h()[2] = data.c()[7] * state[1];
    }
    

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

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

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

  template <typename Scalar_>
  struct amb_traits<ActuatorDCFirstOrderMotorModel<Scalar_> >
  {
    typedef  ActuatorDCFirstOrderMotorModel<Scalar_> ActuatorModelDerived;
    typedef  ActuatorDCFirstOrderMotorData<Scalar_> ActuatorDataDerived;
  };


} // end of namespace se3

#endif /* _se3_dc_linear_first_order_motor_model_ */