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