TwoDofControllerPDModel.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
2 
12 #include <iostream>
13 #include <cmath>
14 
15 #define NUM_CONVOLUTION_TERM 3
16 
18  param = TwoDofControllerPDModel::TwoDofControllerPDModelParam(); // use default constructor
19  current_time = 0;
20  convolutions.clear();
21  for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
22  convolutions.push_back(Convolution(0.0, 0.0));
23  }
24  error_prefix = ""; // inheritted from TwoDofControllerInterface
25 }
26 
28  param.ke = _param.ke; param.kd = _param.kd; param.tc = _param.tc; param.dt = _param.dt;
29  current_time = 0;
30  convolutions.clear();
31  for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
32  convolutions.push_back(Convolution(_param.dt, _range));
33  }
34  error_prefix = ""; // inheritted from TwoDofControllerInterface
35 }
36 
38 }
39 
41  param.ke = 0; param.kd = 0; param.tc = 0; param.dt = 0;
42  convolutions.clear();
43  reset();
44 }
45 
47  param.ke = _param.ke; param.kd = _param.kd; param.tc = _param.tc; param.dt = _param.dt;
48  convolutions.clear();
49  for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
50  convolutions.push_back(Convolution(_param.dt, _range));
51  }
52  reset();
53 }
54 
56  return false;
57 }
58 
60  _p.ke = param.ke;
61  _p.kd = param.kd;
62  _p.tc = param.tc;
63  _p.dt = param.dt;
64  return true;
65 }
66 
68  current_time = 0;
69  for (std::vector<Convolution>::iterator itr = convolutions.begin(); itr != convolutions.end(); ++itr) {
70  (*itr).reset();
71  }
72 }
73 
74 double TwoDofControllerPDModel::update (double _x, double _xd) {
75  // motor model: P = -ke / s + kd
76 
77  double velocity; // velocity calcurated by 2 dof controller
78 
79  // check parameters
80  if (!param.ke || !param.kd || !param.tc || !param.dt) {
81  std::cerr << "[" << error_prefix << "]" << "TwoDofControllerPDModel parameters are not set." << std::endl;
82  std::cerr << "[" << error_prefix << "]" << "ke: " << param.ke << ", kd: " << param.kd << ", tc: " << param.tc << ", dt: " << param.dt << std::endl;
83  return 0;
84  }
85 
86  // update convolution
87  convolutions[0].update(std::exp((param.ke / param.kd) * current_time), _x);
88  convolutions[1].update(std::exp((param.ke / param.kd) * current_time), _xd - _x);
89  convolutions[2].update(1 - std::exp((param.ke / param.kd) * current_time), _xd - _x);
90 
91  // 2 dof controller
92  velocity = (1 / (param.tc * param.kd)) * (-convolutions[0].calculate() + convolutions[1].calculate())
93  - (1 / (param.tc * param.tc * param.ke)) * convolutions[2].calculate();
94 
96 
97  return velocity * param.dt;
98 
99 }
png_uint_32 i
std::vector< Convolution > convolutions
TwoDofControllerPDModelParam param
double update(double _x, double _xd)
Feedback and Feedforward Controller which use PDModel as motor model.
#define NUM_CONVOLUTION_TERM


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