MotorTorqueController.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
2 
11 #include "MotorTorqueController.h"
12 // #include "hrpsys/util/Hrpsys.h"
13 #include <iostream>
14 #include <cmath>
15 #include <boost/version.hpp>
16 #if BOOST_VERSION >= 103500
17 #include <boost/math/special_functions/sign.hpp>
18 #endif
19 #include <typeinfo>
20 
21 #define TRANSITION_TIME 2.0 // [sec]
22 #define MAX_TRANSITION_COUNT (TRANSITION_TIME/m_dt)
23 #define TORQUE_MARGIN 10.0 // [Nm]
24 #define DEFAULT_MIN_MAX_DQ 0.26 // default min/max is 15[deg] = 0.26[rad]
25 #define DEFAULT_MIN_MAX_TRANSITION_DQ (0.17 * m_dt) // default min/max is 10[deg/sec] = 0.17[rad/sec]
26 
28 {
29  // default constructor: _jname = "", _ke = _tc = _dt = 0.0
31  param.ke = 0.0; param.tc = 0.0; param.dt = 0.0;
32  setupController(param);
33  setupControllerCommon("", param.dt);
36 
37 }
38 
40 {
41 }
42 
43 // for TwoDofController
45 {
46  setupController(_param);
47  setupControllerCommon(_jname, _param.dt);
50 }
51 
53 {
57 }
58 
60 {
62  bool retval;
63  retval = m_normalController.getTwoDofControllerParam(_param); // assuming normalController and emergencyController has same parameters
64  return retval;
65  } else {
66  std::cerr << "motor model type is not TwoDofController" << std::endl;
67  return false;
68  }
69 }
70 
72 {
74  bool retval;
76  retval = m_emergencyController.updateTwoDofControllerParam(_param) && retval;
77  return retval;
78  } else {
79  std::cerr << "motor model type is not TwoDofController" << std::endl;
80  return false;
81  }
82 }
83 
84 // for TwoDofControllerPDModel
86 {
87  setupController(_param);
88  setupControllerCommon(_jname, _param.dt);
91 
92 }
94 {
98 }
99 
101 {
103  bool retval;
104  retval = m_normalController.updateTwoDofControllerPDModelParam(_param); // assuming normalController and emergencyController has same parameters
105  return retval;
106  } else {
107  std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerPDModel" << std::endl;
108  return false;
109  }
110 }
111 
113 {
115  bool retval;
118  return retval;
119  } else {
120  std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerPDModel" << std::endl;
121  return false;
122  }
123 }
124 
125 // for TwoDofControllerDynamicsModel
127 {
128  setupController(_param);
129  setupControllerCommon(_jname, _param.dt);
132 }
133 
135 {
139 }
140 
142 {
144  bool retval;
145  retval = m_normalController.getTwoDofControllerDynamiccsModelParam(_param); // assuming normalController and emergencyController has same parameters
146  return retval;
147  } else {
148  std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerDynamicsModel" << std::endl;
149  return false;
150  }
151 }
152 
154 {
156  bool retval;
159  return retval;
160  } else {
161  std::cerr << "[" << m_error_prefix << "]" << "motor model type is not TwoDofControllerDynamicsModel" << std::endl;
162  return false;
163  }
164 }
165 
166 // common public functions
168 {
169  m_enable_flag = true;
170  return true; // return result of changing mode
171 }
172 
174 {
175  bool retval;
177  std::cerr << "[" << m_error_prefix << "]" << "Normal torque control in " << m_joint_name << " is active" << std::endl;
178  retval = false;
179  } else if (m_emergencyController.state != INACTIVE) {
180  std::cerr << "[" << m_error_prefix << "]" << "Emergency torque control in " << m_joint_name << " is active" << std::endl;
181  retval = false;
182  } else{
183  m_enable_flag = false;
184  retval = true;
185  }
186  return retval; // return result of changing mode
187 }
188 
190 {
191  m_normalController.min_dq = _min_dq;
192  m_emergencyController.min_dq = _min_dq;
193  m_normalController.max_dq = _max_dq;
194  m_emergencyController.max_dq = _max_dq;
195 
196  return;
197 }
198 
199 void MotorTorqueController::setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq)
200 {
201  m_normalController.min_transition_dq = _min_transition_dq;
202  m_emergencyController.min_transition_dq = _min_transition_dq;
203  m_normalController.max_transition_dq = _max_transition_dq;
204  m_emergencyController.max_transition_dq = _max_transition_dq;
205 
206  return;
207 }
208 
210 {
211  bool retval = false;
216  retval = true;
217  } else {
218  std::cerr << "[ERROR] Torque control in " << m_joint_name << " is already active" << std::endl;
219  retval = false;
220  }
221  return retval;
222 }
223 
225 {
227  return true;
228 }
229 
231 {
232  m_command_tauRef = _tauRef;
233  return true;
234 }
235 
236 double MotorTorqueController::execute (double _tau, double _tauMax)
237 {
238  double dq, limitedTauRef;
239 
240  if (!m_enable_flag) {
241  return 0.0; // dq = 0.0 when disabled
242  }
243 
244  // define emergency state
245  if (std::abs(_tau) > std::abs(_tauMax)) {
247  // save transtion of current controller
250  } else if (m_normalController.state != INACTIVE) {
252  }
256  }
257  } else {
259  std::abs(_tau) <= std::max(std::abs(_tauMax) - TORQUE_MARGIN, 0.0)) {
260  if (m_normalController.state != INACTIVE) { // take control over normal process
263  } else { // activate stop process for emergency
265  }
266  }
267  }
268 
269  // execute torque control and renew state
270  limitedTauRef = std::min(std::max(-_tauMax, m_command_tauRef), _tauMax);
271  updateController(_tau, limitedTauRef, m_normalController);
273  if (m_emergencyController.state != INACTIVE) { // overwrite by tauMax control when emergency mode
274 #if BOOST_VERSION >= 103500
275  limitedTauRef = boost::math::copysign(_tauMax, _tau);
276 #else
277  limitedTauRef = std::fabs(_tauMax) * ((_tau < 0) ? -1 : 1);
278 #endif
279  updateController(_tau, limitedTauRef, m_emergencyController);
281  }
282 
283  // for debug
284  m_current_tau = _tau;
285  m_actual_tauRef = limitedTauRef;
286 
287  return dq;
288 }
289 
291 {
292  return m_joint_name;
293 }
294 
296 {
298  return m_normalController.state;
299  } else {
301  }
302 }
303 
305 {
306  return m_enable_flag;
307 }
308 
309 void MotorTorqueController::setErrorPrefix(const std::string& _error_prefix)
310 {
311  m_error_prefix = _error_prefix;
312  m_emergencyController.setErrorPrefix(_error_prefix);
313  m_normalController.setErrorPrefix(_error_prefix);
314 }
315 
317 {
318  std::string prefix = "[" + m_error_prefix + "]";
319  prefix += m_joint_name + ".";
320  std::cerr << prefix << "normalController.state:" << m_normalController.state << std::endl;
321  std::cerr << prefix << "normalController.dq:" << m_normalController.getMotorControllerDq() << std::endl;
322  std::cerr << prefix << "emergencyController.state:" << m_emergencyController.state << std::endl;
323  std::cerr << prefix << "emergencyController.dq:" << m_emergencyController.getMotorControllerDq() << std::endl;
324  std::cerr << prefix << "tau:" << m_current_tau << std::endl;
325  std::cerr << prefix << "command_tauRef:" << m_command_tauRef << std::endl;
326  std::cerr << prefix << "actual_tauRef:" << m_actual_tauRef << std::endl;
327  std::cerr << std::endl;
328 }
329 
331 {
332  return m_motor_model_type;
333 }
334 
335 // internal functions
336 void MotorTorqueController::setupControllerCommon(std::string _jname, double _dt)
337 {
338  m_joint_name = _jname;
339  m_dt = _dt;
340  m_command_tauRef = 0.0;
341  m_actual_tauRef = 0.0;
346  m_error_prefix = "";
347  m_enable_flag = false;
348 }
349 
351 {
352  _mc.dq = 0;
353  _mc.transition_dq = 0;
354  _mc.recovery_dq = 0;
355 }
356 
358 {
359  // angle difference to be recoverd
361 
362  // determine transition in 1 cycle
363  _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
364  std::cerr << _mc.recovery_dq << std::endl;
365 
366  _mc.dq = 0; // dq must be reseted after recovery_dq setting(used in getMotoroControllerDq)
367  _mc.state = STOP;
368  return;
369 }
370 
372 {
373  switch (_mc.state) {
374  case ACTIVE:
375  _mc.dq += _mc.controller->update(_tau, _tauRef);
376  _mc.dq = std::min(std::max(_mc.min_dq, _mc.dq), _mc.max_dq);
377  break;
378  case STOP:
379  if (std::abs(_mc.recovery_dq) >= std::abs(_mc.transition_dq)){
380  _mc.dq = 0;
381  _mc.transition_dq = 0;
382  _mc.state = INACTIVE;
383  break;
384  }
385  _mc.transition_dq -= _mc.recovery_dq;
386  break;
387  default:
388  _mc.controller->reset();
390  break;
391  }
392  return;
393 }
394 
395 // for MotorController
397 {
398  state = INACTIVE;
399  dq = 0;
400  transition_dq = 0;
401  recovery_dq = 0;
403  param.ke = 0.0; param.tc = 0.0; param.dt = 0.0;
404  setupTwoDofController(param);
405  error_prefix = "";
406 }
407 
409 {
410 }
411 
413 {
414  controller.reset(new TwoDofController(_param));
415  controller->reset();
416 }
417 
419 {
420  if (typeid(*controller) != typeid(TwoDofController) || boost::dynamic_pointer_cast<TwoDofController>(controller) == NULL) {
421  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofController" << std::endl;
422  return false;
423  }
425  (boost::dynamic_pointer_cast<TwoDofController>(controller))->getParameter(param);
426  updateParam(_param.ke, param.ke);
427  updateParam(_param.tc, param.tc);
428  updateParam(_param.dt, param.dt);
429  return true;
430 }
431 
433 {
434  if (typeid(*controller) != typeid(TwoDofController) || boost::dynamic_pointer_cast<TwoDofController>(controller) == NULL) {
435  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofController" << std::endl;
436  return false;
437  }
438  if (state != INACTIVE) {
439  std::cerr << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
440  return false;
441  }
442  // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
444  (boost::dynamic_pointer_cast<TwoDofController>(controller))->getParameter(param);
445  updateParam(param.ke, _param.ke);
446  updateParam(param.tc, _param.tc);
447  updateParam(param.dt, _param.dt);
448  (boost::dynamic_pointer_cast<TwoDofController>(controller))->setup(param);
449  return true;
450 }
451 
453 {
454  controller.reset(new TwoDofControllerPDModel(_param));
455  controller->reset();
456 }
457 
459 {
460  if (typeid(*controller) != typeid(TwoDofControllerPDModel) || boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller) == NULL) {
461  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerPDModel" << std::endl;
462  return false;
463  }
464  // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
466  (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->getParameter(param);
467  updateParam(_param.ke, param.ke);
468  updateParam(_param.kd, param.kd);
469  updateParam(_param.tc, param.tc);
470  updateParam(_param.dt, param.dt);
471  return true;
472 }
473 
475 {
476  if (typeid(*controller) != typeid(TwoDofControllerPDModel) || boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller) == NULL) {
477  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerPDModel" << std::endl;
478  return false;
479  }
480  if (state != INACTIVE) {
481  std::cerr << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
482  return false;
483  }
484  // update parameters which are not 0 using updateParam (parameter is not updated when _param is 0)
486  (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->getParameter(param);
487  updateParam(param.ke, _param.ke);
488  updateParam(param.kd, _param.kd);
489  updateParam(param.tc, _param.tc);
490  updateParam(param.dt, _param.dt);
491  (boost::dynamic_pointer_cast<TwoDofControllerPDModel>(controller))->setup(param);
492  return true;
493 }
494 
496 {
497  controller.reset(new TwoDofControllerDynamicsModel(_param));
498  controller->reset();
499 }
500 
502 {
503  if (typeid(*controller) != typeid(TwoDofControllerDynamicsModel) || boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller) == NULL) {
504  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerDynamicsModel" << std::endl;
505  return false;
506  }
508  (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->getParameter(param);
509  updateParam(_param.alpha, param.alpha);
510  updateParam(_param.beta, param.beta);
511  updateParam(_param.ki, param.ki);
512  updateParam(_param.tc, param.tc);
513  updateParam(_param.dt, param.dt);
514  return true;
515 }
516 
518 {
519  if (typeid(*controller) != typeid(TwoDofControllerDynamicsModel) || boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller) == NULL) {
520  std::cerr << "[" << error_prefix << "]" << "incorrect controller type: TwoDofControllerDynamicsModel" << std::endl;
521  return false;
522  }
523  if (state != INACTIVE) {
524  std::cerr << "[" << error_prefix << "]" << "controller is not inactive" << std::endl;
525  return false;
526  }
528  (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->getParameter(param);
529  updateParam(param.alpha, _param.alpha);
530  updateParam(param.beta, _param.beta);
531  updateParam(param.ki, _param.ki);
532  updateParam(param.tc, _param.tc);
533  updateParam(param.dt, _param.dt);
534  (boost::dynamic_pointer_cast<TwoDofControllerDynamicsModel>(controller))->setup(param);
535  return true;
536 }
537 
538 bool MotorTorqueController::MotorController::updateParam(double &_param, const double &_new_value)
539 {
540  if (_new_value != 0) { // update parameter if given value is not 0 (new_value = 0 express holding existent parameter)
541  _param = _new_value;
542  return true;
543  }
544  return false;
545 }
546 
547 void MotorTorqueController::MotorController::setErrorPrefix(const std::string& _error_prefix)
548 {
549  error_prefix = _error_prefix;
550  controller->setErrorPrefix(_error_prefix);
551 }
552 
554 {
555  double ret_dq;
556  switch(state) {
557  case ACTIVE:
558  ret_dq = dq + transition_dq; // if contorller interrupt its transition, base joint angle is not qRef, qRef + transition_dq
559  break;
560  case STOP:
561  ret_dq = transition_dq;
562  break;
563  default:
564  ret_dq = dq;
565  break;
566  }
567  return ret_dq;
568 }
bool getTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
#define max(a, b)
double execute(double _tau, double _tauMax)
void setupControllerCommon(std::string _jname, double _dt)
void prepareStop(MotorController &_mc)
bool updateControllerParam(TwoDofController::TwoDofControllerParam &_param)
void updateController(double _tau, double _tauRef, MotorController &_mc)
state
controller_state_t getMotorControllerState(void)
#define DEFAULT_MIN_MAX_DQ
bool setReferenceTorque(double _tauRef)
bool updateParam(double &_param, const double &_new_value)
#define DEFAULT_MIN_MAX_TRANSITION_DQ
void setErrorPrefix(const std::string &_error_prefix)
#define min(a, b)
void setupTwoDofControllerPDModel(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
bool getTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
void setupMotorControllerTransitionMinMaxDq(double _min_transition_dq, double _max_transition_dq)
motor_model_t getMotorModelType(void)
bool updateTwoDofControllerDynamiccsModelParam(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
bool getControllerParam(TwoDofController::TwoDofControllerParam &_param)
#define MAX_TRANSITION_COUNT
void setupMotorControllerControlMinMaxDq(double _min_dq, double _max_dq)
bool updateTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param)
bool updateTwoDofControllerPDModelParam(TwoDofControllerPDModel::TwoDofControllerPDModelParam &_param)
MotorController m_emergencyController
MotorController m_normalController
void setupTwoDofController(TwoDofController::TwoDofControllerParam &_param)
torque controller for single motor
#define TORQUE_MARGIN
void setErrorPrefix(const std::string &_error_prefix)
void resetMotorControllerVariables(MotorController &_mc)
boost::shared_ptr< TwoDofControllerInterface > controller
void setupController(TwoDofController::TwoDofControllerParam &_param)
void setupTwoDofControllerDynamicsModel(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param)
bool getTwoDofControllerParam(TwoDofController::TwoDofControllerParam &_param)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20