pneumatic-two-chambers.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_pneumatic_two_chambers_hpp_
#define _se3_pneumatic_two_chambers_hpp_

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


namespace se3
{
  template<typename Scalar_> 
  class PneumaticTwoChambersData :
    ActuatorDataBase< PneumaticTwoChambersData<Scalar_> >
  {
  public:
    typedef Scalar_ Scalar_t;
    typedef typename Eigen::Matrix<Scalar_, 12,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_, 1,1> X_t;
    typedef typename Eigen::Matrix<Scalar_, 1,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_VALVE_VOLTAGE_ORIGIN,
  P_VALVE_VOLTAGE_SCALE,
  P_KINK_FACTOR,
  P_DS_DDV,
  P_DR_DDV,
  P_VOLUME_SENSOR_SCALE,
  P_B_EQ,
  P_RATE_ASSYMMETRY,
  P_LEAKAGE,
  P_PRESSURE_SENSOR_BIAS,
  P_PRESSURE_SENSOR_SCALE,
  P_VOLTAGE_RANGE_OF_LEAKY_REGIME
      };

    void valveVoltageOrigin(Scalar_ c)
    { c_[P_VALVE_VOLTAGE_ORIGIN] = c;}

    void valveVoltageScale(Scalar_ c)
    { c_[P_VALVE_VOLTAGE_SCALE]=c;}
    
    void kinkFactor(Scalar_ c)
    { c_[P_KINK_FACTOR]=c;}
    
    void pDsDdv(Scalar_ c) 
    {c_[P_DS_DDV] = c;}
    
    void pDrDdv(Scalar_ c)
    {c_[P_DR_DDV] = c;}
    
    void pBeq(Scalar_ c) 
    {c_[P_B_EQ] = c;}

    void pRateAssymmetry(Scalar_ c)
    {c[P_RATE_ASSYMMETRY] = c;}

    void pLeakage(Scalar_ c)
    {c[P_LEAKAGE] = c;}

    void pPressureSensorBias(Scalar_ c)
    {c[P_PRESSURE_SENSOR_BIAS] = c;}

    void pPressureSensorScale(Scalar_ c)
    {c[P_PRESSURE_SENSOR_SCALE] = c;}

    // \brief Voltage Range of leaky regime
    void pVoltageRangeOfLeakyRegime(Scalar_ c)
    {c[P_VOLTAGE_RANGE_OF_LEAKY_REGIME] = c; }

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

    Observations_t & h() { return h_;}
    
  protected:

    Observations_t h_;
    
    Parameters_t c_;
    
    S_t S_;
    
  };

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

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

    void ode_func(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
      
      // 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(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_ */