MotorTorqueController.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef MOTOR_TORQUE_CONTROLLER_H
00011 #define MOTOR_TORQUE_CONTROLLER_H
00012 
00013 #include <string>
00014 #include <boost/shared_ptr.hpp>
00015 #include "../Stabilizer/TwoDofController.h"
00016 #include "TwoDofControllerPDModel.h"
00017 #include "TwoDofControllerDynamicsModel.h"
00018 
00019 // </rtc-template>
00020 
00021 class MotorTorqueController {
00022 public:
00023   enum motor_model_t {
00024     TWO_DOF_CONTROLLER,
00025     TWO_DOF_CONTROLLER_PD_MODEL,
00026     TWO_DOF_CONTROLLER_DYNAMICS_MODEL
00027   };
00028 
00029   enum controller_state_t {
00030     INACTIVE, // dq = 0
00031     STOP, // resume
00032     ACTIVE // execute torque control
00033   };
00034 
00035   MotorTorqueController();
00036   ~MotorTorqueController(void);
00037 
00038   // for TwoDofController
00039   MotorTorqueController(std::string _jname, TwoDofController::TwoDofControllerParam &_param);
00040   void setupController(TwoDofController::TwoDofControllerParam &_param);
00041   bool getControllerParam(TwoDofController::TwoDofControllerParam &_param);
00042   bool updateControllerParam(TwoDofController::TwoDofControllerParam &_param);
00043   // for TwoDofControllerPDModel
00044   MotorTorqueController(std::string _jname, TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00045   void setupController(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00046   bool getControllerParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00047   bool updateControllerParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00048   // for TwoDofControllerDynamicsModel
00049   MotorTorqueController(std::string _jname, TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00050   void setupController(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00051   bool getControllerParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00052   bool updateControllerParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00053 
00054   // for normal/emergency torque contorller
00055   bool enable(void); // enable torque controller (normal controller is not activated but emergency toruqe control may be activated)
00056   bool disable(void); // disable torque controller (emergency controller is also ignored)
00057 
00058   // for normal torque controller
00059   void setupMotorControllerControlMinMaxDq(double _min_dq, double _max_dq); // set min/max dq for control
00060   void setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq); // set min/max dq for transition
00061   bool activate(void); // set state of torque controller to ACTIVE
00062   bool deactivate(void); // set state of torque controller to STOP -> INACTIVE
00063   bool setReferenceTorque(double _tauRef); // set reference torque (does not activate controller)
00064 
00065   double execute(double _tau, double _tauMax); // determine final state and tauRef, then throw tau, tauRef and state to executeControl
00066   
00067   // accessor
00068   motor_model_t getMotorModelType(void);
00069   std::string getJointName(void);
00070   controller_state_t getMotorControllerState(void);
00071   bool isEnabled(void);
00072 
00073   // for debug
00074   void setErrorPrefix(const std::string& _error_prefix);
00075   void printMotorControllerVariables(void); // debug print
00076   
00077 private:
00078   class MotorController {
00079   public:
00080     MotorController();
00081     ~MotorController();
00082     boost::shared_ptr<TwoDofControllerInterface> controller;
00083     controller_state_t state;
00084     double dq; //difference of joint angle from base(qRef) from tdc. it is calcurated by dq = integrate(qd * dt), dq*dt is output of tdc 
00085     double transition_dq; // for transition. first value is last difference of joint angle from qRef (dq + transition_dq) when state was changed to STOP
00086     double recovery_dq; // difference of joint angle in 1 cycle to be recoverd
00087     double min_dq; // min total dq when control
00088     double max_dq; // max total dq when control
00089     double min_transition_dq; // min dq when transition
00090     double max_transition_dq; // max dq when transition
00091 
00092     // for TwoDofController
00093     void setupTwoDofController(TwoDofController::TwoDofControllerParam &_param);
00094     bool getTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param);
00095     bool updateTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param);
00096     // for TwoDofControllerPDModel
00097     void setupTwoDofControllerPDModel(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00098     bool getTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00099     bool updateTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param);
00100     // for TwoDofControllerDynamicsModel
00101     void setupTwoDofControllerDynamicsModel(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00102     bool getTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00103     bool updateTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param);
00104     double getMotorControllerDq(void); // get according dq according to state
00105     void setErrorPrefix(const std::string& _error_prefix);
00106   private:
00107     bool updateParam(double &_param, const double &_new_value); // update param if new_value is acceptable
00108     std::string error_prefix;
00109   };
00110   
00111   // internal functions
00112   void setupControllerCommon(std::string _jname, double _dt);
00113   void resetMotorControllerVariables(MotorController& _mc); // reset internal torque control parameter  
00114   void prepareStop(MotorController &_mc);
00115   void updateController(double _tau, double _tauRef, MotorController& _mc); // execute control and update controller member valiables 
00116   
00117   std::string m_joint_name; // joint name which is controled
00118   motor_model_t m_motor_model_type; // motor model type which is used
00119   int m_transition_count; // positive value when stopping
00120   double m_dt; // control term
00121   double m_current_tau; // current tau (mainly for debug message)
00122   double m_command_tauRef; // reference tau
00123   double m_actual_tauRef; // reference tau which is limited or overwritten by emergency (mainly for debug message)
00124   MotorController m_normalController; // substance of two dof controller
00125   MotorController m_emergencyController; // overwrite normal controller when emergency
00126   std::string m_error_prefix; // assumed to be instance name of rtc
00127   bool m_enable_flag;
00128 };
00129 
00130 
00131 #endif // MOTOR_TORQUE_CONTROLLER_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55