MotorTorqueController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00011 #include "MotorTorqueController.h"
00012 // #include "hrpsys/util/Hrpsys.h"
00013 #include <iostream>
00014 #include <cmath>
00015 #include <boost/version.hpp>
00016 #if BOOST_VERSION >= 103500
00017 #include <boost/math/special_functions/sign.hpp>
00018 #endif
00019 #include <typeinfo>
00020 
00021 #define TRANSITION_TIME 2.0 // [sec]
00022 #define MAX_TRANSITION_COUNT (TRANSITION_TIME/m_dt)
00023 #define TORQUE_MARGIN 10.0 // [Nm]
00024 #define DEFAULT_MIN_MAX_DQ 0.26 // default min/max is 15[deg] = 0.26[rad]
00025 #define DEFAULT_MIN_MAX_TRANSITION_DQ (0.17 * m_dt) // default min/max is 10[deg/sec] = 0.17[rad/sec]
00026 
00027 MotorTorqueController::MotorTorqueController()
00028 {
00029   // default constructor: _jname = "", _ke = _tc = _dt = 0.0
00030   TwoDofController::TwoDofControllerParam param;
00031   param.ke = 0.0; param.tc = 0.0; param.dt = 0.0;
00032   setupController(param);
00033   setupControllerCommon("", param.dt);
00034   setupMotorControllerControlMinMaxDq(0.0, 0.0);
00035   setupMotorControllerTransitionMinMaxDq(0.0, 0.0); 
00036 
00037 }
00038 
00039 MotorTorqueController::~MotorTorqueController(void)
00040 {
00041 }
00042 
00043 // for TwoDofController
00044 MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofController::TwoDofControllerParam &_param)
00045 {
00046   setupController(_param);
00047   setupControllerCommon(_jname, _param.dt);
00048   setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
00049   setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ); 
00050 }
00051 
00052 void MotorTorqueController::setupController(TwoDofController::TwoDofControllerParam &_param)
00053 {
00054   m_motor_model_type = TWO_DOF_CONTROLLER;
00055   m_normalController.setupTwoDofController(_param);
00056   m_emergencyController.setupTwoDofController(_param);
00057 }
00058 
00059 bool MotorTorqueController::getControllerParam(TwoDofController::TwoDofControllerParam &_param)
00060 {
00061   if (m_motor_model_type == TWO_DOF_CONTROLLER) {
00062     bool retval;
00063     retval = m_normalController.getTwoDofControllerParam(_param); // assuming normalController and emergencyController has same parameters
00064     return retval;
00065   } else {
00066     std::cerr << "motor model type is not TwoDofController" << std::endl;
00067     return false;
00068   }
00069 }
00070 
00071 bool MotorTorqueController::updateControllerParam(TwoDofController::TwoDofControllerParam &_param)
00072 {
00073   if (m_motor_model_type == TWO_DOF_CONTROLLER) {
00074     bool retval;
00075     retval = m_normalController.updateTwoDofControllerParam(_param);
00076     retval = m_emergencyController.updateTwoDofControllerParam(_param) && retval;
00077     return retval;
00078   } else {
00079     std::cerr << "motor model type is not TwoDofController" << std::endl;
00080     return false;
00081   }
00082 }
00083 
00084 // for TwoDofControllerPDModel
00085 MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00086 {
00087   setupController(_param);
00088   setupControllerCommon(_jname, _param.dt);
00089   setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
00090   setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ); 
00091 
00092 }
00093 void MotorTorqueController::setupController(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00094 {
00095   m_motor_model_type = TWO_DOF_CONTROLLER_PD_MODEL;
00096   m_normalController.setupTwoDofControllerPDModel(_param);
00097   m_emergencyController.setupTwoDofControllerPDModel(_param);
00098 }
00099 
00100 bool MotorTorqueController::getControllerParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00101 {
00102   if (m_motor_model_type == TWO_DOF_CONTROLLER_PD_MODEL) {
00103     bool retval;
00104     retval = m_normalController.updateTwoDofControllerPDModelParam(_param); // assuming normalController and emergencyController has same parameters
00105     return retval;
00106   } else {
00107     std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerPDModel" << std::endl;
00108     return false;
00109   }
00110 }
00111 
00112 bool MotorTorqueController::updateControllerParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00113 {
00114   if (m_motor_model_type == TWO_DOF_CONTROLLER_PD_MODEL) {
00115     bool retval;
00116     retval = m_normalController.updateTwoDofControllerPDModelParam(_param);
00117     retval = m_emergencyController.updateTwoDofControllerPDModelParam(_param) && retval;
00118     return retval;
00119   } else {
00120     std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerPDModel" << std::endl;
00121     return false;
00122   }
00123 }
00124 
00125 // for TwoDofControllerDynamicsModel
00126 MotorTorqueController::MotorTorqueController(std::string _jname, TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00127 {
00128   setupController(_param);
00129   setupControllerCommon(_jname, _param.dt);
00130   setupMotorControllerControlMinMaxDq(-DEFAULT_MIN_MAX_DQ, DEFAULT_MIN_MAX_DQ);
00131   setupMotorControllerTransitionMinMaxDq(-DEFAULT_MIN_MAX_TRANSITION_DQ, DEFAULT_MIN_MAX_TRANSITION_DQ); 
00132 }
00133 
00134 void MotorTorqueController::setupController(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00135 {
00136   m_motor_model_type = TWO_DOF_CONTROLLER_DYNAMICS_MODEL;
00137   m_normalController.setupTwoDofControllerDynamicsModel(_param);
00138   m_emergencyController.setupTwoDofControllerDynamicsModel(_param);
00139 }
00140 
00141 bool MotorTorqueController::getControllerParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00142 {
00143   if (m_motor_model_type == TWO_DOF_CONTROLLER_DYNAMICS_MODEL) {
00144     bool retval;
00145     retval = m_normalController.getTwoDofControllerDynamiccsModelParam(_param); // assuming normalController and emergencyController has same parameters
00146     return retval;
00147   } else {
00148     std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerDynamicsModel" << std::endl;
00149     return false;
00150   }
00151 }
00152 
00153 bool MotorTorqueController::updateControllerParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00154 {
00155   if (m_motor_model_type == TWO_DOF_CONTROLLER_DYNAMICS_MODEL) {
00156     bool retval;
00157     retval = m_normalController.updateTwoDofControllerDynamiccsModelParam(_param);
00158     retval = m_emergencyController.updateTwoDofControllerDynamiccsModelParam(_param) && retval;
00159     return retval;
00160   } else {
00161     std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerDynamicsModel" << std::endl;
00162     return false;
00163   }
00164 }
00165 
00166 // common public functions
00167 bool MotorTorqueController::enable(void)
00168 {
00169   m_enable_flag = true;
00170   return true; // return result of changing mode 
00171 }
00172 
00173 bool MotorTorqueController::disable(void)
00174 {
00175   bool retval;
00176   if (m_normalController.state != INACTIVE) {
00177     std::cerr << "[" << m_error_prefix << "]" << "Normal torque control in " << m_joint_name << " is active" << std::endl;
00178     retval = false;
00179   } else if (m_emergencyController.state != INACTIVE) {
00180     std::cerr << "[" << m_error_prefix << "]" << "Emergency torque control in " << m_joint_name << " is active" << std::endl;
00181     retval = false;
00182   } else{
00183     m_enable_flag = false;
00184     retval = true;
00185   }
00186   return retval; // return result of changing mode
00187 }
00188 
00189 void MotorTorqueController::setupMotorControllerControlMinMaxDq(double _min_dq, double _max_dq)
00190 {
00191   m_normalController.min_dq = _min_dq;
00192   m_emergencyController.min_dq = _min_dq;
00193   m_normalController.max_dq = _max_dq;
00194   m_emergencyController.max_dq = _max_dq;
00195 
00196   return;
00197 }
00198 
00199 void MotorTorqueController::setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq)
00200 {
00201   m_normalController.min_transition_dq = _min_transition_dq;
00202   m_emergencyController.min_transition_dq = _min_transition_dq;
00203   m_normalController.max_transition_dq = _max_transition_dq;
00204   m_emergencyController.max_transition_dq = _max_transition_dq;
00205   
00206   return;
00207 }
00208 
00209 bool MotorTorqueController::activate(void)
00210 {
00211   bool retval = false;
00212   if (m_normalController.state == INACTIVE) {
00213     resetMotorControllerVariables(m_normalController);
00214     m_normalController.controller->reset();
00215     m_normalController.state = ACTIVE;
00216     retval = true;
00217   } else {
00218     std::cerr << "[ERROR] Torque control in " << m_joint_name << " is already active" << std::endl;
00219     retval = false;
00220   }
00221   return retval;
00222 }
00223 
00224 bool MotorTorqueController::deactivate(void)
00225 {
00226   prepareStop(m_normalController);
00227   return true;
00228 }
00229 
00230 bool MotorTorqueController::setReferenceTorque(double _tauRef)
00231 {
00232   m_command_tauRef = _tauRef;
00233   return true;
00234 }
00235 
00236 double MotorTorqueController::execute (double _tau, double _tauMax)
00237 {
00238   double dq, limitedTauRef;
00239 
00240   if (!m_enable_flag) {
00241     return 0.0; // dq = 0.0 when disabled
00242   }
00243  
00244   // define emergency state
00245   if (std::abs(_tau) > std::abs(_tauMax)) {
00246     if (m_emergencyController.state != ACTIVE) {
00247       // save transtion of current controller 
00248       if (m_emergencyController.state != INACTIVE) {
00249         m_emergencyController.transition_dq = m_emergencyController.getMotorControllerDq();
00250       } else if (m_normalController.state != INACTIVE) {
00251         m_emergencyController.transition_dq = m_normalController.getMotorControllerDq();
00252       }
00253       m_emergencyController.dq = 0;
00254       m_emergencyController.controller->reset();
00255       m_emergencyController.state = ACTIVE;
00256     }
00257   } else {
00258     if (m_emergencyController.state == ACTIVE &&
00259         std::abs(_tau) <= std::max(std::abs(_tauMax) - TORQUE_MARGIN, 0.0)) {
00260       if (m_normalController.state != INACTIVE) { // take control over normal process
00261         m_normalController.transition_dq = m_emergencyController.getMotorControllerDq();
00262         m_emergencyController.state = INACTIVE;
00263       } else { // activate stop process for emergency
00264         prepareStop(m_emergencyController);
00265       }
00266     }
00267   }
00268 
00269   // execute torque control and renew state
00270   limitedTauRef = std::min(std::max(-_tauMax, m_command_tauRef), _tauMax);
00271   updateController(_tau, limitedTauRef, m_normalController);
00272   dq = m_normalController.getMotorControllerDq();
00273   if (m_emergencyController.state != INACTIVE) { // overwrite by tauMax control when emergency mode
00274 #if BOOST_VERSION >= 103500
00275     limitedTauRef = boost::math::copysign(_tauMax, _tau);
00276 #else
00277     limitedTauRef = std::fabs(_tauMax) * ((_tau < 0) ? -1 : 1);
00278 #endif
00279     updateController(_tau, limitedTauRef, m_emergencyController);
00280     dq = m_emergencyController.getMotorControllerDq();
00281   }
00282 
00283   // for debug
00284   m_current_tau = _tau;
00285   m_actual_tauRef = limitedTauRef;
00286   
00287   return dq;
00288 }
00289 
00290 std::string MotorTorqueController::getJointName(void)
00291 {
00292   return m_joint_name;
00293 }
00294 
00295 MotorTorqueController::controller_state_t MotorTorqueController::getMotorControllerState(void)
00296 {
00297   if (m_emergencyController.state == INACTIVE) {
00298     return m_normalController.state;
00299   } else {
00300     return m_emergencyController.state;
00301   }
00302 }
00303 
00304 bool MotorTorqueController::isEnabled(void)
00305 {
00306   return m_enable_flag;
00307 }
00308 
00309 void MotorTorqueController::setErrorPrefix(const std::string& _error_prefix)
00310 {
00311   m_error_prefix = _error_prefix;
00312   m_emergencyController.setErrorPrefix(_error_prefix);
00313   m_normalController.setErrorPrefix(_error_prefix);
00314 }
00315 
00316 void MotorTorqueController::printMotorControllerVariables(void)
00317 {
00318   std::string prefix = "[" + m_error_prefix + "]";
00319   prefix += m_joint_name + ".";
00320   std::cerr << prefix << "normalController.state:" << m_normalController.state  << std::endl;
00321   std::cerr << prefix << "normalController.dq:" << m_normalController.getMotorControllerDq()  << std::endl;
00322   std::cerr << prefix << "emergencyController.state:" << m_emergencyController.state  << std::endl;
00323   std::cerr << prefix << "emergencyController.dq:" << m_emergencyController.getMotorControllerDq() << std::endl;
00324   std::cerr << prefix << "tau:" << m_current_tau  << std::endl;
00325   std::cerr << prefix << "command_tauRef:" << m_command_tauRef  << std::endl;
00326   std::cerr << prefix << "actual_tauRef:" << m_actual_tauRef  << std::endl;
00327   std::cerr << std::endl;
00328 }
00329 
00330 MotorTorqueController::motor_model_t MotorTorqueController::getMotorModelType(void)
00331 {
00332   return m_motor_model_type;
00333 }
00334 
00335 // internal functions
00336 void MotorTorqueController::setupControllerCommon(std::string _jname, double _dt)
00337 {
00338   m_joint_name = _jname;
00339   m_dt = _dt;
00340   m_command_tauRef = 0.0;
00341   m_actual_tauRef = 0.0;
00342   m_normalController.state = INACTIVE;
00343   resetMotorControllerVariables(m_normalController);
00344   m_emergencyController.state = INACTIVE;
00345   resetMotorControllerVariables(m_emergencyController);
00346   m_error_prefix = "";
00347   m_enable_flag = false;
00348 }
00349 
00350 void MotorTorqueController::resetMotorControllerVariables(MotorTorqueController::MotorController& _mc)
00351 {
00352   _mc.dq = 0;
00353   _mc.transition_dq = 0;
00354   _mc.recovery_dq = 0;
00355 }
00356 
00357 void MotorTorqueController::prepareStop(MotorTorqueController::MotorController &_mc)
00358 {
00359   // angle difference to be recoverd
00360   _mc.transition_dq = _mc.getMotorControllerDq();
00361 
00362   // determine transition in 1 cycle
00363   _mc.recovery_dq = std::min(std::max(_mc.transition_dq / MAX_TRANSITION_COUNT, _mc.min_transition_dq), _mc.max_transition_dq); // transition in 1 cycle
00364   std::cerr << _mc.recovery_dq << std::endl;
00365   
00366   _mc.dq = 0; // dq must be reseted after recovery_dq setting(used in getMotoroControllerDq)
00367   _mc.state = STOP;
00368   return;
00369 }
00370 
00371 void MotorTorqueController::updateController(double _tau, double _tauRef, MotorTorqueController::MotorController& _mc)
00372 {
00373   switch (_mc.state) {
00374   case ACTIVE:
00375     _mc.dq += _mc.controller->update(_tau, _tauRef);
00376     _mc.dq = std::min(std::max(_mc.min_dq, _mc.dq), _mc.max_dq);
00377     break;
00378   case STOP:
00379     if (std::abs(_mc.recovery_dq) >= std::abs(_mc.transition_dq)){
00380         _mc.dq = 0;
00381         _mc.transition_dq = 0;
00382         _mc.state = INACTIVE;
00383         break;
00384       }
00385     _mc.transition_dq -= _mc.recovery_dq;
00386     break;
00387   default:
00388     _mc.controller->reset();
00389     resetMotorControllerVariables(_mc);
00390     break;
00391   }
00392   return;
00393 }
00394 
00395 // for MotorController
00396 MotorTorqueController::MotorController::MotorController()
00397 {
00398   state = INACTIVE;
00399   dq = 0;
00400   transition_dq = 0;
00401   recovery_dq = 0;
00402   TwoDofController::TwoDofControllerParam param;
00403   param.ke = 0.0; param.tc = 0.0; param.dt = 0.0;
00404   setupTwoDofController(param);
00405   error_prefix = "";
00406 }
00407 
00408 MotorTorqueController::MotorController::~MotorController()
00409 {
00410 }
00411 
00412 void MotorTorqueController::MotorController::setupTwoDofController(TwoDofController::TwoDofControllerParam &_param)
00413 {
00414   controller.reset(new TwoDofController(_param));
00415   controller->reset();
00416 }
00417 
00418 bool MotorTorqueController::MotorController::getTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param)
00419 {
00420   if (typeid(*controller) != typeid(TwoDofController) || boost::dynamic_pointer_cast<TwoDofController>(controller) == NULL) {
00421     std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofController" << std::endl;
00422     return false;
00423   }
00424   TwoDofController::TwoDofControllerParam param;
00425   (boost::dynamic_pointer_cast<TwoDofController>(controller))->getParameter(param);
00426   updateParam(_param.ke, param.ke);
00427   updateParam(_param.tc, param.tc);
00428   updateParam(_param.dt, param.dt);
00429   return true;
00430 }
00431 
00432 bool MotorTorqueController::MotorController::updateTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param)
00433 {
00434   if (typeid(*controller) != typeid(TwoDofController) || boost::dynamic_pointer_cast<TwoDofController>(controller) == NULL) {
00435     std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofController" << std::endl;
00436     return false;
00437   }  
00438   if (state != INACTIVE) {
00439     std::cerr << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
00440     return false;
00441   }
00442   // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
00443   TwoDofController::TwoDofControllerParam param;
00444   (boost::dynamic_pointer_cast<TwoDofController>(controller))->getParameter(param);
00445   updateParam(param.ke, _param.ke);
00446   updateParam(param.tc, _param.tc);
00447   updateParam(param.dt, _param.dt);
00448   (boost::dynamic_pointer_cast<TwoDofController>(controller))->setup(param);
00449   return true;
00450 }
00451 
00452 void MotorTorqueController::MotorController::setupTwoDofControllerPDModel(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00453 {
00454   controller.reset(new TwoDofControllerPDModel(_param));
00455   controller->reset();
00456 }
00457 
00458 bool MotorTorqueController::MotorController::getTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00459 {
00460   if (typeid(*controller) != typeid(TwoDofControllerPDModel) || boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller) == NULL) {
00461     std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerPDModel" << std::endl;
00462     return false;
00463   }  
00464   // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
00465   TwoDofControllerPDModel::TwoDofControllerPDModelParam param;
00466   (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->getParameter(param);
00467   updateParam(_param.ke, param.ke);
00468   updateParam(_param.kd, param.kd);
00469   updateParam(_param.tc, param.tc);
00470   updateParam(_param.dt, param.dt);
00471   return true;
00472 }
00473 
00474 bool MotorTorqueController::MotorController::updateTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
00475 {
00476   if (typeid(*controller) != typeid(TwoDofControllerPDModel) || boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller) == NULL) {
00477     std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerPDModel" << std::endl;
00478     return false;
00479   }  
00480   if (state != INACTIVE) {
00481     std::cerr << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
00482     return false;
00483   }
00484   // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
00485   TwoDofControllerPDModel::TwoDofControllerPDModelParam param;
00486   (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->getParameter(param);
00487   updateParam(param.ke, _param.ke);
00488   updateParam(param.kd, _param.kd);
00489   updateParam(param.tc, _param.tc);
00490   updateParam(param.dt, _param.dt);
00491   (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->setup(param);
00492   return true;
00493 }
00494 
00495 void MotorTorqueController::MotorController::setupTwoDofControllerDynamicsModel(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00496 {
00497   controller.reset(new TwoDofControllerDynamicsModel(_param));
00498   controller->reset();
00499 }
00500 
00501 bool MotorTorqueController::MotorController::getTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00502 {
00503   if (typeid(*controller) != typeid(TwoDofControllerDynamicsModel) || boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller) == NULL) {
00504     std::cerr  << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerDynamicsModel" << std::endl;
00505     return false;
00506   }
00507   TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam param;
00508   (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->getParameter(param);
00509   updateParam(_param.alpha, param.alpha);
00510   updateParam(_param.beta, param.beta);
00511   updateParam(_param.ki, param.ki);
00512   updateParam(_param.tc, param.tc);
00513   updateParam(_param.dt, param.dt);
00514   return true;
00515 }
00516 
00517 bool MotorTorqueController::MotorController::updateTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
00518 {
00519   if (typeid(*controller) != typeid(TwoDofControllerDynamicsModel) || boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller) == NULL) {
00520     std::cerr  << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerDynamicsModel" << std::endl;
00521     return false;
00522   }
00523   if (state != INACTIVE) {
00524     std::cerr  << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
00525     return false;
00526   }
00527   TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam param;
00528   (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->getParameter(param);
00529   updateParam(param.alpha, _param.alpha);
00530   updateParam(param.beta, _param.beta);
00531   updateParam(param.ki, _param.ki);
00532   updateParam(param.tc, _param.tc);
00533   updateParam(param.dt, _param.dt);
00534   (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->setup(param);
00535   return true;
00536 }
00537 
00538 bool MotorTorqueController::MotorController::updateParam(double &_param, const double &_new_value)
00539 {
00540   if (_new_value != 0) { // update parameter if given value is not 0 (new_value = 0 express holding existent parameter) 
00541     _param = _new_value;
00542     return true;
00543   }
00544   return false;
00545 }
00546 
00547 void MotorTorqueController::MotorController::setErrorPrefix(const std::string& _error_prefix)
00548 {
00549   error_prefix = _error_prefix;
00550   controller->setErrorPrefix(_error_prefix);
00551 }
00552 
00553 double MotorTorqueController::MotorController::getMotorControllerDq(void)
00554 {
00555   double ret_dq;
00556   switch(state) {
00557   case ACTIVE:
00558     ret_dq = dq + transition_dq; // if contorller interrupt its transition, base joint angle is not qRef, qRef + transition_dq
00559     break;
00560   case STOP:
00561     ret_dq = transition_dq;
00562     break;
00563   default:
00564     ret_dq = dq;
00565     break;
00566   }
00567   return ret_dq;
00568 }


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