sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
motor-model.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #include <cmath>
7 #include <cassert>
11 namespace dynamicgraph {
12 namespace sot {
13 namespace torque_control {
15 
16 double MotorModel::getCurrent(double torque, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p, double Kf_n,
17  double Kv_p, double Kv_n, double Ka_p, double Ka_n, unsigned int poly) {
18  assert(Kt_p > 0.0 && "Kt_p should be > 0");
19  assert(Kt_n > 0.0 && "Kt_n should be > 0");
20  assert(Kf_p >= 0.0 && "Kf_p should be >= 0");
21  assert(Kf_n >= 0.0 && "Kf_n should be >= 0");
22  assert(Kv_p >= 0.0 && "Kv_p should be >= 0");
23  assert(Kv_n >= 0.0 && "Kv_n should be >= 0");
24  assert(Ka_p >= 0.0 && "Ka_p should be >= 0");
25  assert(Ka_n >= 0.0 && "Ka_n should be >= 0");
26 
27  double signDq = this->smoothSign(dq, 0.1, poly); // in [-1;1]
28  double current;
29 
30  // Smoothly set Coefficients according to velocity sign
31  double Kt = 0.5 * (Kt_p * (1 + signDq) + Kt_n * (1 - signDq));
32  double Kv = 0.5 * (Kv_p * (1 + signDq) + Kv_n * (1 - signDq));
33  double Ka = 0.5 * (Ka_p * (1 + signDq) + Ka_n * (1 - signDq));
34  double Kf = 0.5 * (Kf_p * (1 + signDq) + Kf_n * (1 - signDq));
35 
36  current = Kt * torque + Kv * dq + Ka * ddq + signDq * Kf;
37 
38  return current;
39 }
40 
41 double MotorModel::getTorque(double current, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p, double Kf_n,
42  double Kv_p, double Kv_n, double Ka_p, double Ka_n, unsigned int poly) {
43  assert(Kt_p > 0.0 && "Kt_p should be > 0");
44  assert(Kt_n > 0.0 && "Kt_n should be > 0");
45  assert(Kf_p >= 0.0 && "Kf_p should be >= 0");
46  assert(Kf_n >= 0.0 && "Kf_n should be >= 0");
47  assert(Kv_p >= 0.0 && "Kv_p should be >= 0");
48  assert(Kv_n >= 0.0 && "Kv_n should be >= 0");
49  assert(Ka_p >= 0.0 && "Ka_p should be >= 0");
50  assert(Ka_n >= 0.0 && "Ka_n should be >= 0");
51 
52  double signDq = this->smoothSign(dq, 0.1, poly); // in [-1;1]
53  double torque;
54 
55  // Smoothly set Coefficients according to velocity sign
56  double Kt = 0.5 * (Kt_p * (1 + signDq) + Kt_n * (1 - signDq));
57  double Kv = 0.5 * (Kv_p * (1 + signDq) + Kv_n * (1 - signDq));
58  double Ka = 0.5 * (Ka_p * (1 + signDq) + Ka_n * (1 - signDq));
59  double Kf = 0.5 * (Kf_p * (1 + signDq) + Kf_n * (1 - signDq));
60 
61  torque = (current / Kt) - (Kv / Kt) * dq - (Ka / Kt) * ddq - signDq * (Kf / Kt);
62 
63  return torque;
64 }
65 
66 double MotorModel::smoothSign(double value, double threshold, unsigned int poly) {
67  if (value > threshold)
68  return 1.0;
69  else if (value < -threshold)
70  return -1.0;
71  double a = value / threshold;
72  if (poly == 1) return a;
73  if (poly == 2 && value > 0) return a * a;
74  if (poly == 2 && value <= 0) return -a * a;
75  return a * a * a;
76 }
77 } // namespace torque_control
78 } // namespace sot
79 } // namespace dynamicgraph
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::MotorModel::smoothSign
double smoothSign(double value, double threshold, unsigned int poly=3)
Definition: motor-model.cpp:66
motor-model.hh
dynamicgraph::sot::torque_control::MotorModel::getTorque
double getTorque(double current, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p=0.0, double Kf_n=0.0, double Kv_p=0.0, double Kv_n=0.0, double Ka_p=0.0, double Ka_n=0.0, unsigned int poly=3)
Definition: motor-model.cpp:41
dynamicgraph::sot::torque_control::MotorModel::getCurrent
double getCurrent(double torque, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p=0.0, double Kf_n=0.0, double Kv_p=0.0, double Kv_n=0.0, double Ka_p=0.0, double Ka_n=0.0, unsigned int poly=3)
Definition: motor-model.cpp:16
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::MotorModel::MotorModel
MotorModel()
Definition: motor-model.cpp:14