hebiros_gazebo_controller.cpp
Go to the documentation of this file.
1 #include <hebiros_gazebo_controller.h>
2 
3 namespace controller {
4 
5 enum class control_strategies {
6  CONTROL_STRATEGY_OFF = 0,
7  CONTROL_STRATEGY_DIRECT_PWM = 1,
8  CONTROL_STRATEGY_2 = 2,
9  CONTROL_STRATEGY_3 = 3,
10  CONTROL_STRATEGY_4 = 4
11 };
12 
13 static constexpr control_strategies DEFAULT_CONTROL_STRATEGY = control_strategies::CONTROL_STRATEGY_3;
14 
15 static constexpr double MAX_PWM = 1.0;
16 static constexpr double MIN_PWM = -1.0;
17 
18 static constexpr double LOW_PASS_ALPHA = 0.1;
19 
20 static constexpr double DEFAULT_POSITION_KP = 0.5;
21 static constexpr double DEFAULT_POSITION_KI = 0.0;
22 static constexpr double DEFAULT_POSITION_KD = 0.0;
23 static constexpr double DEFAULT_VELOCITY_KP = 0.05;
24 static constexpr double DEFAULT_VELOCITY_KI = 0.0;
25 static constexpr double DEFAULT_VELOCITY_KD = 0.0;
26 static constexpr double DEFAULT_EFFORT_KP = 0.25;
27 static constexpr double DEFAULT_EFFORT_KI = 0.0;
28 static constexpr double DEFAULT_EFFORT_KD = 0.001;
29 
30 static constexpr double GEAR_RATIO_X5_1 = 272.22;
31 static constexpr double GEAR_RATIO_X8_3 = 272.22;
32 static constexpr double GEAR_RATIO_X5_4 = 762.22;
33 static constexpr double GEAR_RATIO_X8_9 = 762.22;
34 static constexpr double GEAR_RATIO_X5_9 = 1742.22;
35 static constexpr double GEAR_RATIO_X8_16 = 1462.222;
36 
37 static constexpr double DEFAULT_GEAR_RATIO = 272.22;
38 
39 static std::map<std::string, double> gear_ratios = {
40  {"X5_1", GEAR_RATIO_X5_1},
41  {"X5_4", GEAR_RATIO_X5_4},
42  {"X5_9", GEAR_RATIO_X5_9},
43  {"X8_3", GEAR_RATIO_X8_3},
44  {"X8_9", GEAR_RATIO_X8_9},
45  {"X8_16", GEAR_RATIO_X8_16}};
46 }
47 
48 using namespace controller;
49 
50 
51 //Set defaults settings for a joint once
52 void HebirosGazeboController::SetSettings(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
53  std::shared_ptr<HebirosGazeboJoint> hebiros_joint) {
54 
55  hebiros_group->settings.name.push_back(hebiros_joint->name);
56  hebiros_joint->low_pass_alpha = LOW_PASS_ALPHA;
57  int i = hebiros_joint->command_index;
58 
59  //Set gear ratio
60  if (gear_ratios.find(hebiros_joint->model_name) != gear_ratios.end()) {
61  hebiros_joint->gear_ratio = gear_ratios[hebiros_joint->model_name];
62  }
63  else {
64  hebiros_joint->gear_ratio = DEFAULT_GEAR_RATIO;
65  }
66 
67  //Set control strategy
68  hebiros_group->settings.control_strategy.push_back(
69  static_cast<char>(DEFAULT_CONTROL_STRATEGY));
70 
71  SetDefaultGains(hebiros_group, hebiros_joint);
72 }
73 
74 //Initialize gains with default values based on model and control strategy
75 void HebirosGazeboController::SetDefaultGains(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
76  std::shared_ptr<HebirosGazeboJoint> hebiros_joint) {
77 
78  std::string model_name = hebiros_joint->model_name;
79  int i = hebiros_joint->command_index;
80  int control_strategy = hebiros_group->settings.control_strategy[i];
81 
82  if (model_name == "X5_1" && control_strategy == 2) {
83  hebiros_group->settings.position_gains.kp.push_back(5);
84  hebiros_group->settings.position_gains.ki.push_back(0);
85  hebiros_group->settings.position_gains.kd.push_back(0);
86  hebiros_group->settings.velocity_gains.kp.push_back(0.1);
87  hebiros_group->settings.velocity_gains.ki.push_back(0);
88  hebiros_group->settings.velocity_gains.kd.push_back(0);
89  hebiros_group->settings.effort_gains.kp.push_back(0.25);
90  hebiros_group->settings.effort_gains.ki.push_back(0);
91  hebiros_group->settings.effort_gains.kd.push_back(0.001);
92  }
93  else if (model_name == "X5_1" && control_strategy == 3) {
94  hebiros_group->settings.position_gains.kp.push_back(0.5);
95  hebiros_group->settings.position_gains.ki.push_back(0);
96  hebiros_group->settings.position_gains.kd.push_back(0);
97  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
98  hebiros_group->settings.velocity_gains.ki.push_back(0);
99  hebiros_group->settings.velocity_gains.kd.push_back(0);
100  hebiros_group->settings.effort_gains.kp.push_back(0.25);
101  hebiros_group->settings.effort_gains.ki.push_back(0);
102  hebiros_group->settings.effort_gains.kd.push_back(0.001);
103  }
104  else if (model_name == "X5_1" && control_strategy == 4) {
105  hebiros_group->settings.position_gains.kp.push_back(5);
106  hebiros_group->settings.position_gains.ki.push_back(0);
107  hebiros_group->settings.position_gains.kd.push_back(0);
108  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
109  hebiros_group->settings.velocity_gains.ki.push_back(0);
110  hebiros_group->settings.velocity_gains.kd.push_back(0);
111  hebiros_group->settings.effort_gains.kp.push_back(0.25);
112  hebiros_group->settings.effort_gains.ki.push_back(0);
113  hebiros_group->settings.effort_gains.kd.push_back(0.001);
114  }
115  else if (model_name == "X5_4" && control_strategy == 2) {
116  hebiros_group->settings.position_gains.kp.push_back(10);
117  hebiros_group->settings.position_gains.ki.push_back(0);
118  hebiros_group->settings.position_gains.kd.push_back(0);
119  hebiros_group->settings.velocity_gains.kp.push_back(0.2);
120  hebiros_group->settings.velocity_gains.ki.push_back(0);
121  hebiros_group->settings.velocity_gains.kd.push_back(0);
122  hebiros_group->settings.effort_gains.kp.push_back(0.25);
123  hebiros_group->settings.effort_gains.ki.push_back(0);
124  hebiros_group->settings.effort_gains.kd.push_back(0.001);
125  }
126  else if (model_name == "X5_4" && control_strategy == 3) {
127  hebiros_group->settings.position_gains.kp.push_back(1);
128  hebiros_group->settings.position_gains.ki.push_back(0);
129  hebiros_group->settings.position_gains.kd.push_back(0);
130  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
131  hebiros_group->settings.velocity_gains.ki.push_back(0);
132  hebiros_group->settings.velocity_gains.kd.push_back(0);
133  hebiros_group->settings.effort_gains.kp.push_back(0.25);
134  hebiros_group->settings.effort_gains.ki.push_back(0);
135  hebiros_group->settings.effort_gains.kd.push_back(0.001);
136  }
137  else if (model_name == "X5_4" && control_strategy == 4) {
138  hebiros_group->settings.position_gains.kp.push_back(10);
139  hebiros_group->settings.position_gains.ki.push_back(0);
140  hebiros_group->settings.position_gains.kd.push_back(0);
141  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
142  hebiros_group->settings.velocity_gains.ki.push_back(0);
143  hebiros_group->settings.velocity_gains.kd.push_back(0);
144  hebiros_group->settings.effort_gains.kp.push_back(0.25);
145  hebiros_group->settings.effort_gains.ki.push_back(0);
146  hebiros_group->settings.effort_gains.kd.push_back(0.001);
147  }
148  else if (model_name == "X5_9" && control_strategy == 2) {
149  hebiros_group->settings.position_gains.kp.push_back(15);
150  hebiros_group->settings.position_gains.ki.push_back(0);
151  hebiros_group->settings.position_gains.kd.push_back(0);
152  hebiros_group->settings.velocity_gains.kp.push_back(0.5);
153  hebiros_group->settings.velocity_gains.ki.push_back(0);
154  hebiros_group->settings.velocity_gains.kd.push_back(0);
155  hebiros_group->settings.effort_gains.kp.push_back(0.25);
156  hebiros_group->settings.effort_gains.ki.push_back(0);
157  hebiros_group->settings.effort_gains.kd.push_back(0.001);
158  }
159  else if (model_name == "X5_9" && control_strategy == 3) {
160  hebiros_group->settings.position_gains.kp.push_back(1.5);
161  hebiros_group->settings.position_gains.ki.push_back(0);
162  hebiros_group->settings.position_gains.kd.push_back(0);
163  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
164  hebiros_group->settings.velocity_gains.ki.push_back(0);
165  hebiros_group->settings.velocity_gains.kd.push_back(0);
166  hebiros_group->settings.effort_gains.kp.push_back(0.25);
167  hebiros_group->settings.effort_gains.ki.push_back(0);
168  hebiros_group->settings.effort_gains.kd.push_back(0.001);
169  }
170  else if (model_name == "X5_9" && control_strategy == 4) {
171  hebiros_group->settings.position_gains.kp.push_back(15);
172  hebiros_group->settings.position_gains.ki.push_back(0);
173  hebiros_group->settings.position_gains.kd.push_back(0);
174  hebiros_group->settings.velocity_gains.kp.push_back(0.05);
175  hebiros_group->settings.velocity_gains.ki.push_back(0);
176  hebiros_group->settings.velocity_gains.kd.push_back(0);
177  hebiros_group->settings.effort_gains.kp.push_back(0.25);
178  hebiros_group->settings.effort_gains.ki.push_back(0);
179  hebiros_group->settings.effort_gains.kd.push_back(0.001);
180  }
181  if (model_name == "X8_3" && control_strategy == 2) {
182  hebiros_group->settings.position_gains.kp.push_back(3);
183  hebiros_group->settings.position_gains.ki.push_back(0);
184  hebiros_group->settings.position_gains.kd.push_back(0);
185  hebiros_group->settings.velocity_gains.kp.push_back(0.1);
186  hebiros_group->settings.velocity_gains.ki.push_back(0);
187  hebiros_group->settings.velocity_gains.kd.push_back(0);
188  hebiros_group->settings.effort_gains.kp.push_back(0.1);
189  hebiros_group->settings.effort_gains.ki.push_back(0);
190  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
191  }
192  else if (model_name == "X8_3" && control_strategy == 3) {
193  hebiros_group->settings.position_gains.kp.push_back(1);
194  hebiros_group->settings.position_gains.ki.push_back(0);
195  hebiros_group->settings.position_gains.kd.push_back(0);
196  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
197  hebiros_group->settings.velocity_gains.ki.push_back(0);
198  hebiros_group->settings.velocity_gains.kd.push_back(0);
199  hebiros_group->settings.effort_gains.kp.push_back(0.1);
200  hebiros_group->settings.effort_gains.ki.push_back(0);
201  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
202  }
203  else if (model_name == "X8_3" && control_strategy == 4) {
204  hebiros_group->settings.position_gains.kp.push_back(3);
205  hebiros_group->settings.position_gains.ki.push_back(0);
206  hebiros_group->settings.position_gains.kd.push_back(0);
207  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
208  hebiros_group->settings.velocity_gains.ki.push_back(0);
209  hebiros_group->settings.velocity_gains.kd.push_back(0);
210  hebiros_group->settings.effort_gains.kp.push_back(0.1);
211  hebiros_group->settings.effort_gains.ki.push_back(0);
212  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
213  }
214  else if (model_name == "X8_9" && control_strategy == 2) {
215  hebiros_group->settings.position_gains.kp.push_back(5);
216  hebiros_group->settings.position_gains.ki.push_back(0);
217  hebiros_group->settings.position_gains.kd.push_back(0);
218  hebiros_group->settings.velocity_gains.kp.push_back(0.1);
219  hebiros_group->settings.velocity_gains.ki.push_back(0);
220  hebiros_group->settings.velocity_gains.kd.push_back(0);
221  hebiros_group->settings.effort_gains.kp.push_back(0.1);
222  hebiros_group->settings.effort_gains.ki.push_back(0);
223  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
224  }
225  else if (model_name == "X8_9" && control_strategy == 3) {
226  hebiros_group->settings.position_gains.kp.push_back(2);
227  hebiros_group->settings.position_gains.ki.push_back(0);
228  hebiros_group->settings.position_gains.kd.push_back(0);
229  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
230  hebiros_group->settings.velocity_gains.ki.push_back(0);
231  hebiros_group->settings.velocity_gains.kd.push_back(0);
232  hebiros_group->settings.effort_gains.kp.push_back(0.1);
233  hebiros_group->settings.effort_gains.ki.push_back(0);
234  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
235  }
236  else if (model_name == "X8_9" && control_strategy == 4) {
237  hebiros_group->settings.position_gains.kp.push_back(5);
238  hebiros_group->settings.position_gains.ki.push_back(0);
239  hebiros_group->settings.position_gains.kd.push_back(0);
240  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
241  hebiros_group->settings.velocity_gains.ki.push_back(0);
242  hebiros_group->settings.velocity_gains.kd.push_back(0);
243  hebiros_group->settings.effort_gains.kp.push_back(0.1);
244  hebiros_group->settings.effort_gains.ki.push_back(0);
245  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
246  }
247  else if (model_name == "X8_16" && control_strategy == 2) {
248  hebiros_group->settings.position_gains.kp.push_back(5);
249  hebiros_group->settings.position_gains.ki.push_back(0);
250  hebiros_group->settings.position_gains.kd.push_back(0);
251  hebiros_group->settings.velocity_gains.kp.push_back(0.1);
252  hebiros_group->settings.velocity_gains.ki.push_back(0);
253  hebiros_group->settings.velocity_gains.kd.push_back(0);
254  hebiros_group->settings.effort_gains.kp.push_back(0.1);
255  hebiros_group->settings.effort_gains.ki.push_back(0);
256  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
257  }
258  else if (model_name == "X8_16" && control_strategy == 3) {
259  hebiros_group->settings.position_gains.kp.push_back(3);
260  hebiros_group->settings.position_gains.ki.push_back(0);
261  hebiros_group->settings.position_gains.kd.push_back(0);
262  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
263  hebiros_group->settings.velocity_gains.ki.push_back(0);
264  hebiros_group->settings.velocity_gains.kd.push_back(0);
265  hebiros_group->settings.effort_gains.kp.push_back(0.1);
266  hebiros_group->settings.effort_gains.ki.push_back(0);
267  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
268  }
269  else if (model_name == "X8_16" && control_strategy == 4) {
270  hebiros_group->settings.position_gains.kp.push_back(5);
271  hebiros_group->settings.position_gains.ki.push_back(0);
272  hebiros_group->settings.position_gains.kd.push_back(0);
273  hebiros_group->settings.velocity_gains.kp.push_back(0.03);
274  hebiros_group->settings.velocity_gains.ki.push_back(0);
275  hebiros_group->settings.velocity_gains.kd.push_back(0);
276  hebiros_group->settings.effort_gains.kp.push_back(0.1);
277  hebiros_group->settings.effort_gains.ki.push_back(0);
278  hebiros_group->settings.effort_gains.kd.push_back(0.0001);
279  }
280  else {
281  hebiros_group->settings.position_gains.kp.push_back(DEFAULT_POSITION_KP);
282  hebiros_group->settings.position_gains.ki.push_back(DEFAULT_POSITION_KI);
283  hebiros_group->settings.position_gains.kd.push_back(DEFAULT_POSITION_KD);
284  hebiros_group->settings.velocity_gains.kp.push_back(DEFAULT_VELOCITY_KP);
285  hebiros_group->settings.velocity_gains.ki.push_back(DEFAULT_VELOCITY_KI);
286  hebiros_group->settings.velocity_gains.kd.push_back(DEFAULT_VELOCITY_KD);
287  hebiros_group->settings.effort_gains.kp.push_back(DEFAULT_EFFORT_KP);
288  hebiros_group->settings.effort_gains.ki.push_back(DEFAULT_EFFORT_KI);
289  hebiros_group->settings.effort_gains.kd.push_back(DEFAULT_EFFORT_KD);
290  }
291 }
292 
293 //Change settings for a joint if specifically commanded
294 void HebirosGazeboController::ChangeSettings(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
295  std::shared_ptr<HebirosGazeboJoint> hebiros_joint) {
296 
297  CommandMsg target = hebiros_group->command_target;
298  int i = hebiros_joint->command_index;
299 
300  //Set name
301  if (i < target.settings.name.size()) {
302  hebiros_group->settings.name[i] = target.settings.name[i];
303  }
304 
305  //Change control strategy
306  if (i < target.settings.control_strategy.size()) {
307  hebiros_group->settings.control_strategy[i] = target.settings.control_strategy[i];
308  }
309 
310  //Change position gains
311  if (i < target.settings.position_gains.kp.size()) {
312  hebiros_group->settings.position_gains.kp[i] = target.settings.position_gains.kp[i];
313  }
314  if (i < target.settings.position_gains.ki.size()) {
315  hebiros_group->settings.position_gains.ki[i] = target.settings.position_gains.ki[i];
316  }
317  if (i < target.settings.position_gains.kd.size()) {
318  hebiros_group->settings.position_gains.kd[i] = target.settings.position_gains.kd[i];
319  }
320 
321  //Change velocity gains
322  if (i < target.settings.velocity_gains.kp.size()) {
323  hebiros_group->settings.velocity_gains.kp[i] = target.settings.velocity_gains.kp[i];
324  }
325  if (i < target.settings.velocity_gains.ki.size()) {
326  hebiros_group->settings.velocity_gains.ki[i] = target.settings.velocity_gains.ki[i];
327  }
328  if (i < target.settings.velocity_gains.kd.size()) {
329  hebiros_group->settings.velocity_gains.kd[i] = target.settings.velocity_gains.kd[i];
330  }
331 
332  //Change effort gains
333  if (i < target.settings.effort_gains.kp.size()) {
334  hebiros_group->settings.effort_gains.kp[i] = target.settings.effort_gains.kp[i];
335  }
336  if (i < target.settings.effort_gains.ki.size()) {
337  hebiros_group->settings.effort_gains.ki[i] = target.settings.effort_gains.ki[i];
338  }
339  if (i < target.settings.effort_gains.kd.size()) {
340  hebiros_group->settings.effort_gains.kd[i] = target.settings.effort_gains.kd[i];
341  }
342 }
343 
344 //Compute output force to the joint based on PID and control strategy
345 double HebirosGazeboController::ComputeForce(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
346  std::shared_ptr<HebirosGazeboJoint> hebiros_joint,
347  double position, double velocity, double effort, const ros::Duration& iteration_time) {
348 
349  CommandMsg target = hebiros_group->command_target;
350  int i = hebiros_joint->command_index;
351 
352  double target_position, target_velocity, target_effort;
353  double position_pid, velocity_pid, effort_pid;
354  double position_pwm, velocity_pwm, effort_pwm;
355  double intermediate_effort;
356  double gear_ratio, pwm, force, alpha;
357 
358  //Set target positions
359  if (i < target.position.size()) {
360  target_position = target.position[i];
361  }
362  else {
363  target_position = position;
364  }
365  if (i < target.velocity.size()) {
366  target_velocity = target.velocity[i];
367  }
368  else {
369  target_velocity = velocity;
370  }
371  if (i < target.effort.size()) {
372  target_effort = target.effort[i];
373  }
374  else {
375  target_effort = effort;
376  }
377 
378  //Combine forces using selected strategy
379  int control_strategy = hebiros_group->settings.control_strategy[i];
380  switch (control_strategy) {
381  case 0:
382  pwm = 0;
383  break;
384 
385  case 1:
386  pwm = Clip(target_effort, MIN_PWM, MAX_PWM);
387  break;
388 
389  case 2:
390  position_pid =
391  ComputePositionPID(hebiros_group, hebiros_joint, target_position, position, iteration_time);
392  velocity_pid =
393  ComputeVelocityPID(hebiros_group, hebiros_joint, target_velocity, velocity, iteration_time);
394  intermediate_effort = target_effort + position_pid + velocity_pid;
395  effort_pwm = Clip(
396  ComputeEffortPID(hebiros_group, hebiros_joint, intermediate_effort, effort, iteration_time),
397  MIN_PWM, MAX_PWM);
398  pwm = effort_pwm;
399  break;
400 
401  case 3:
402  position_pwm = Clip(
403  ComputePositionPID(hebiros_group, hebiros_joint, target_position, position, iteration_time),
404  MIN_PWM, MAX_PWM);
405  velocity_pwm = Clip(
406  ComputeVelocityPID(hebiros_group, hebiros_joint, target_velocity, velocity, iteration_time),
407  MIN_PWM, MAX_PWM);
408  effort_pwm = Clip(
409  ComputeEffortPID(hebiros_group, hebiros_joint, target_effort, effort, iteration_time),
410  MIN_PWM, MAX_PWM);
411  pwm = Clip(position_pwm + velocity_pwm + effort_pwm, MIN_PWM, MAX_PWM);
412  break;
413 
414  case 4:
415  position_pid =
416  ComputePositionPID(hebiros_group, hebiros_joint, target_position, position, iteration_time);
417  intermediate_effort = target_effort + position_pid;
418  effort_pwm = Clip(
419  ComputeEffortPID(hebiros_group, hebiros_joint, intermediate_effort, effort, iteration_time),
420  MIN_PWM, MAX_PWM);
421  velocity_pwm = Clip(
422  ComputeVelocityPID(hebiros_group, hebiros_joint, target_velocity, velocity, iteration_time),
423  MIN_PWM, MAX_PWM);
424  pwm = Clip(velocity_pwm + effort_pwm, MIN_PWM, MAX_PWM);
425  break;
426 
427  default:
428  pwm = 0;
429  }
430 
431  gear_ratio = hebiros_joint->gear_ratio;
432 
433  float voltage = 48.0f;
434  float motor_velocity = velocity * gear_ratio;
435  float speed_constant = 1530.0f;
436  float term_resist = 9.99f;
437  if (hebiros_joint->isX8()) {
438  speed_constant = 1360.0f;
439  term_resist = 3.19f;
440  }
441 
442  pwm = hebiros_joint->temperature_safety.limit(pwm);
443 
444  if (pwm == 0) {
445  force = 0;
446  }
447  else {
448  // TODO: use temp compensation here, too?
449  force = ((pwm*voltage - (motor_velocity/speed_constant)) / term_resist) * 0.00626 * gear_ratio * 0.65;
450  }
451 
452  float prev_winding_temp = hebiros_joint->temperature.getMotorWindingTemperature();
453 
454  // Get components of power into the motor
455 
456  // Temperature compensated speed constant
457  float comp_speed_constant = speed_constant * 1.05f * // Experimental tuning factor
458  (1.f + .001f * (prev_winding_temp - 20.f)); // .001 is speed constant change per temperature change
459  float winding_resistance = term_resist *
460  (1.f + .004f * (prev_winding_temp - 20.f)); // .004 is resistance change per temperature change for copper
461  float back_emf = (motor_velocity * 30.f / M_PI) / comp_speed_constant;
462  float winding_voltage = pwm * voltage - back_emf;
463 
464  // TODO: could add ripple current estimate here, too
465 
466  // Update temperature:
467  // Power = I^2R, but I = V/R so I^2R = V^2/R:
468  double power_in = winding_voltage * winding_voltage / winding_resistance;
469  hebiros_joint->temperature.update(power_in, iteration_time.toSec());
470  hebiros_joint->temperature_safety.update(hebiros_joint->temperature.getMotorWindingTemperature());
471 
472  //alpha = hebiros_joint->low_pass_alpha;
473  //force = (force * alpha) + hebiros_joint->prev_force * (1 - alpha);
474  //hebiros_joint->prev_force = force;
475 
476  return force;
477 }
478 
479 //Compute the PID error for positions
480 double HebirosGazeboController::ComputePositionPID(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
481  std::shared_ptr<HebirosGazeboJoint> hebiros_joint,
482  double target_position, double position, const ros::Duration& iteration_time) {
483 
484  double position_error_p, position_error_i, position_error_d;
485 
486  position_error_p = target_position - position;
487  position_error_i = hebiros_joint->position_elapsed_error + position_error_p;
488  position_error_d = (position_error_p - hebiros_joint->position_prev_error) /
489  iteration_time.toSec();
490  hebiros_joint->position_prev_error = position_error_p;
491  hebiros_joint->position_elapsed_error = position_error_i;
492 
493  if (iteration_time.toSec() <= 0) {
494  position_error_d = 0;
495  }
496 
497  int i = hebiros_joint->command_index;
498 
499  return (hebiros_group->settings.position_gains.kp[i] * position_error_p) +
500  (hebiros_group->settings.position_gains.ki[i] * position_error_i) +
501  (hebiros_group->settings.position_gains.kd[i] * position_error_d);
502 }
503 
504 //Compute the PID error for velocities
505 double HebirosGazeboController::ComputeVelocityPID(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
506  std::shared_ptr<HebirosGazeboJoint> hebiros_joint,
507  double target_velocity, double velocity, const ros::Duration& iteration_time) {
508 
509  double velocity_error_p, velocity_error_i, velocity_error_d;
510 
511  velocity_error_p = target_velocity - velocity;
512  velocity_error_i = hebiros_joint->velocity_elapsed_error + velocity_error_p;
513  velocity_error_d = (velocity_error_p - hebiros_joint->velocity_prev_error) /
514  iteration_time.toSec();
515  hebiros_joint->velocity_prev_error = velocity_error_p;
516  hebiros_joint->velocity_elapsed_error = velocity_error_i;
517 
518  if (iteration_time.toSec() <= 0) {
519  velocity_error_d = 0;
520  }
521 
522  int i = hebiros_joint->command_index;
523 
524  return (hebiros_group->settings.velocity_gains.kp[i] * velocity_error_p) +
525  (hebiros_group->settings.velocity_gains.ki[i] * velocity_error_i) +
526  (hebiros_group->settings.velocity_gains.kd[i] * velocity_error_d);
527 }
528 
529 //Compute the PID error for efforts
530 double HebirosGazeboController::ComputeEffortPID(std::shared_ptr<HebirosGazeboGroup> hebiros_group,
531  std::shared_ptr<HebirosGazeboJoint> hebiros_joint,
532  double target_effort, double effort, const ros::Duration& iteration_time) {
533 
534  double effort_error_p, effort_error_i, effort_error_d;
535 
536  effort_error_p = target_effort - effort;
537  effort_error_i = hebiros_joint->effort_elapsed_error + effort_error_p;
538  effort_error_d = (effort_error_p - hebiros_joint->effort_prev_error) /
539  iteration_time.toSec();
540  hebiros_joint->effort_prev_error = effort_error_p;
541  hebiros_joint->effort_elapsed_error = effort_error_i;
542 
543  if (iteration_time.toSec() <= 0) {
544  effort_error_d = 0;
545  }
546 
547  int i = hebiros_joint->command_index;
548 
549  return (hebiros_group->settings.effort_gains.kp[i] * effort_error_p) +
550  (hebiros_group->settings.effort_gains.ki[i] * effort_error_i) +
551  (hebiros_group->settings.effort_gains.kd[i] * effort_error_d);
552 }
553 
554 //Limit x to a value from low to high
555 double HebirosGazeboController::Clip(double x, double low, double high) {
556  return std::min(std::max(x, low), high);
557 }
558 
controller::DEFAULT_POSITION_KD
static constexpr double DEFAULT_POSITION_KD
Definition: hebiros_gazebo_controller.cpp:22
controller::DEFAULT_POSITION_KP
static constexpr double DEFAULT_POSITION_KP
Definition: hebiros_gazebo_controller.cpp:20
controller::DEFAULT_CONTROL_STRATEGY
static constexpr control_strategies DEFAULT_CONTROL_STRATEGY
Definition: hebiros_gazebo_controller.cpp:13
HebirosGazeboController::SetDefaultGains
static void SetDefaultGains(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint)
Definition: hebiros_gazebo_controller.cpp:75
controller::DEFAULT_GEAR_RATIO
static constexpr double DEFAULT_GEAR_RATIO
Definition: hebiros_gazebo_controller.cpp:37
controller::DEFAULT_EFFORT_KP
static constexpr double DEFAULT_EFFORT_KP
Definition: hebiros_gazebo_controller.cpp:26
controller::DEFAULT_EFFORT_KD
static constexpr double DEFAULT_EFFORT_KD
Definition: hebiros_gazebo_controller.cpp:28
HebirosGazeboController::ComputeVelocityPID
static double ComputeVelocityPID(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint, double target_velocity, double velocity, const ros::Duration &iteration_time)
Definition: hebiros_gazebo_controller.cpp:505
HebirosGazeboController::ComputePositionPID
static double ComputePositionPID(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint, double target_position, double position, const ros::Duration &iteration_time)
Definition: hebiros_gazebo_controller.cpp:480
HebirosGazeboController::SetSettings
static void SetSettings(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint)
Definition: hebiros_gazebo_controller.cpp:52
controller::GEAR_RATIO_X5_1
static constexpr double GEAR_RATIO_X5_1
Definition: hebiros_gazebo_controller.cpp:30
M_PI
#define M_PI
controller::MAX_PWM
static constexpr double MAX_PWM
Definition: hebiros_gazebo_controller.cpp:15
hebiros_gazebo_controller.h
controller::GEAR_RATIO_X8_16
static constexpr double GEAR_RATIO_X8_16
Definition: hebiros_gazebo_controller.cpp:35
controller::GEAR_RATIO_X8_3
static constexpr double GEAR_RATIO_X8_3
Definition: hebiros_gazebo_controller.cpp:31
controller::DEFAULT_POSITION_KI
static constexpr double DEFAULT_POSITION_KI
Definition: hebiros_gazebo_controller.cpp:21
controller::GEAR_RATIO_X5_4
static constexpr double GEAR_RATIO_X5_4
Definition: hebiros_gazebo_controller.cpp:32
controller::GEAR_RATIO_X8_9
static constexpr double GEAR_RATIO_X8_9
Definition: hebiros_gazebo_controller.cpp:33
controller::DEFAULT_VELOCITY_KI
static constexpr double DEFAULT_VELOCITY_KI
Definition: hebiros_gazebo_controller.cpp:24
HebirosGazeboController::ChangeSettings
static void ChangeSettings(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint)
Definition: hebiros_gazebo_controller.cpp:294
controller::MIN_PWM
static constexpr double MIN_PWM
Definition: hebiros_gazebo_controller.cpp:16
HebirosGazeboController::ComputeEffortPID
static double ComputeEffortPID(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint, double target_effort, double effort, const ros::Duration &iteration_time)
Definition: hebiros_gazebo_controller.cpp:530
controller::DEFAULT_VELOCITY_KP
static constexpr double DEFAULT_VELOCITY_KP
Definition: hebiros_gazebo_controller.cpp:23
controller::DEFAULT_EFFORT_KI
static constexpr double DEFAULT_EFFORT_KI
Definition: hebiros_gazebo_controller.cpp:27
controller::GEAR_RATIO_X5_9
static constexpr double GEAR_RATIO_X5_9
Definition: hebiros_gazebo_controller.cpp:34
HebirosGazeboController::Clip
static double Clip(double x, double low, double high)
Definition: hebiros_gazebo_controller.cpp:555
controller::gear_ratios
static std::map< std::string, double > gear_ratios
Definition: hebiros_gazebo_controller.cpp:39
HebirosGazeboController::ComputeForce
static double ComputeForce(std::shared_ptr< HebirosGazeboGroup > hebiros_group, std::shared_ptr< HebirosGazeboJoint > hebiros_joint, double position, double velocity, double effort, const ros::Duration &iteration_time)
Definition: hebiros_gazebo_controller.cpp:345
controller::LOW_PASS_ALPHA
static constexpr double LOW_PASS_ALPHA
Definition: hebiros_gazebo_controller.cpp:18
controller::DEFAULT_VELOCITY_KD
static constexpr double DEFAULT_VELOCITY_KD
Definition: hebiros_gazebo_controller.cpp:25

hebiros_gazebo_plugin
Author(s): Xavier Artache , Matthew Tesch
autogenerated on Thu Sep 3 2020 04:13:55