1 |
|
|
/* |
2 |
|
|
* Copyright 2015, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <Eigen/Dense> |
9 |
|
|
#include <sot/core/debug.hh> |
10 |
|
|
#include <sot/torque_control/commands-helper.hh> |
11 |
|
|
#include <sot/torque_control/joint-torque-controller.hh> |
12 |
|
|
|
13 |
|
|
namespace dynamicgraph { |
14 |
|
|
namespace sot { |
15 |
|
|
namespace torque_control { |
16 |
|
|
|
17 |
|
|
#define MODEL_INPUT_SIGNALS \ |
18 |
|
|
m_motorParameterKt_pSIN << m_motorParameterKt_nSIN \ |
19 |
|
|
<< m_motorParameterKf_pSIN \ |
20 |
|
|
<< m_motorParameterKf_nSIN \ |
21 |
|
|
<< m_motorParameterKv_pSIN \ |
22 |
|
|
<< m_motorParameterKv_nSIN \ |
23 |
|
|
<< m_motorParameterKa_pSIN \ |
24 |
|
|
<< m_motorParameterKa_nSIN << m_polySignDqSIN |
25 |
|
|
|
26 |
|
|
#define ESTIMATOR_INPUT_SIGNALS \ |
27 |
|
|
m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN \ |
28 |
|
|
<< m_jointsTorquesSIN << m_jointsTorquesDerivativeSIN |
29 |
|
|
|
30 |
|
|
#define TORQUE_INTEGRAL_INPUT_SIGNALS \ |
31 |
|
|
m_KiTorqueSIN << m_torque_integral_saturationSIN |
32 |
|
|
#define TORQUE_CONTROL_INPUT_SIGNALS \ |
33 |
|
|
m_jointsTorquesDesiredSIN << m_KpTorqueSIN << m_KdTorqueSIN \ |
34 |
|
|
<< m_coulomb_friction_compensation_percentageSIN |
35 |
|
|
#define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN << m_KiVelSIN |
36 |
|
|
|
37 |
|
|
#define ALL_INPUT_SIGNALS \ |
38 |
|
|
ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS \ |
39 |
|
|
<< TORQUE_CONTROL_INPUT_SIGNALS \ |
40 |
|
|
<< VEL_CONTROL_INPUT_SIGNALS << MODEL_INPUT_SIGNALS |
41 |
|
|
#define ALL_OUTPUT_SIGNALS \ |
42 |
|
|
m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT |
43 |
|
|
|
44 |
|
|
namespace dynamicgraph = ::dynamicgraph; |
45 |
|
|
using namespace dynamicgraph; |
46 |
|
|
using namespace dynamicgraph::command; |
47 |
|
|
using namespace std; |
48 |
|
|
using namespace Eigen; |
49 |
|
|
|
50 |
|
|
/// Define EntityClassName here rather than in the header file |
51 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
52 |
|
|
typedef JointTorqueController EntityClassName; |
53 |
|
|
|
54 |
|
|
/* --- DG FACTORY ------------------------------------------------------- */ |
55 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTorqueController, |
56 |
|
|
"JointTorqueController"); |
57 |
|
|
|
58 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
59 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
60 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
61 |
|
|
JointTorqueController::JointTorqueController(const std::string& name) |
62 |
|
|
: Entity(name), |
63 |
|
|
CONSTRUCT_SIGNAL_IN(jointsPositions, dynamicgraph::Vector), |
64 |
|
|
CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector), |
65 |
|
|
CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector), |
66 |
|
|
CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector), |
67 |
|
|
CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector), |
68 |
|
|
CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector), |
69 |
|
|
CONSTRUCT_SIGNAL_IN(dq_des, dynamicgraph::Vector), |
70 |
|
|
CONSTRUCT_SIGNAL_IN(KpTorque, |
71 |
|
|
dynamicgraph::Vector) // proportional gain for torque |
72 |
|
|
// feedback controller |
73 |
|
|
, |
74 |
|
|
CONSTRUCT_SIGNAL_IN( |
75 |
|
|
KiTorque, |
76 |
|
|
dynamicgraph::Vector) // integral gain for torque feedback controller |
77 |
|
|
, |
78 |
|
|
CONSTRUCT_SIGNAL_IN(KdTorque, |
79 |
|
|
dynamicgraph::Vector) // derivative gain for torque |
80 |
|
|
// feedback controller |
81 |
|
|
, |
82 |
|
|
CONSTRUCT_SIGNAL_IN( |
83 |
|
|
KdVel, dynamicgraph::Vector) // derivative gain for velocity feedback |
84 |
|
|
, |
85 |
|
|
CONSTRUCT_SIGNAL_IN( |
86 |
|
|
KiVel, dynamicgraph::Vector) // integral gain for velocity feedback |
87 |
|
|
, |
88 |
|
|
CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector), |
89 |
|
|
CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage, |
90 |
|
|
dynamicgraph::Vector), |
91 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector), |
92 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector), |
93 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector), |
94 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector), |
95 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector), |
96 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector), |
97 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector), |
98 |
|
|
CONSTRUCT_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector), |
99 |
|
|
CONSTRUCT_SIGNAL_IN(polySignDq, dynamicgraph::Vector), |
100 |
|
|
CONSTRUCT_SIGNAL_OUT( |
101 |
|
|
u, dynamicgraph::Vector, |
102 |
|
|
ESTIMATOR_INPUT_SIGNALS |
103 |
|
|
<< TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS |
104 |
|
|
<< MODEL_INPUT_SIGNALS << m_torque_error_integralSOUT), |
105 |
|
|
CONSTRUCT_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector, |
106 |
|
|
m_jointsVelocitiesSIN), |
107 |
|
|
CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector, |
108 |
|
|
m_jointsTorquesSIN |
109 |
|
|
<< m_jointsTorquesDesiredSIN |
110 |
|
|
<< TORQUE_INTEGRAL_INPUT_SIGNALS) { |
111 |
|
|
Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS); |
112 |
|
|
|
113 |
|
|
/* Commands. */ |
114 |
|
|
addCommand("getTimestep", |
115 |
|
|
makeDirectGetter(*this, &m_dt, |
116 |
|
|
docDirectGetter("Control timestep", "double"))); |
117 |
|
|
|
118 |
|
|
addCommand("init", |
119 |
|
|
makeCommandVoid2(*this, &JointTorqueController::init, |
120 |
|
|
docCommandVoid2("Initialize the controller.", |
121 |
|
|
"Control timestep [s].", |
122 |
|
|
"Robot reference (string)"))); |
123 |
|
|
addCommand("reset_integral", |
124 |
|
|
makeCommandVoid0(*this, &JointTorqueController::reset_integral, |
125 |
|
|
docCommandVoid0("Reset the integral error."))); |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
129 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
130 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
131 |
|
|
void JointTorqueController::init(const double& timestep, |
132 |
|
|
const std::string& robot_ref) { |
133 |
|
|
assert(timestep > 0.0 && "Timestep should be > 0"); |
134 |
|
|
if (!m_jointsVelocitiesSIN.isPlugged()) |
135 |
|
|
return SEND_MSG("Init failed: signal jointsVelocities is not plugged", |
136 |
|
|
MSG_TYPE_ERROR); |
137 |
|
|
if (!m_jointsTorquesSIN.isPlugged()) |
138 |
|
|
return SEND_MSG("Init failed: signal m_jointsTorquesSIN is not plugged", |
139 |
|
|
MSG_TYPE_ERROR); |
140 |
|
|
if (!m_jointsTorquesDesiredSIN.isPlugged()) |
141 |
|
|
return SEND_MSG( |
142 |
|
|
"Init failed: signal m_jointsTorquesDesiredSIN is not plugged", |
143 |
|
|
MSG_TYPE_ERROR); |
144 |
|
|
if (!m_KpTorqueSIN.isPlugged()) |
145 |
|
|
return SEND_MSG("Init failed: signal m_KpTorqueSIN is not plugged", |
146 |
|
|
MSG_TYPE_ERROR); |
147 |
|
|
if (!m_KiTorqueSIN.isPlugged()) |
148 |
|
|
return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged", |
149 |
|
|
MSG_TYPE_ERROR); |
150 |
|
|
|
151 |
|
|
/* Retrieve m_robot_util informations */ |
152 |
|
|
std::string localName(robot_ref); |
153 |
|
|
if (isNameInRobotUtil(localName)) { |
154 |
|
|
m_robot_util = getRobotUtil(localName); |
155 |
|
|
} else { |
156 |
|
|
SEND_MSG("You should have an entity controller manager initialized before", |
157 |
|
|
MSG_TYPE_ERROR); |
158 |
|
|
return; |
159 |
|
|
} |
160 |
|
|
|
161 |
|
|
m_dt = timestep; |
162 |
|
|
m_tau_star.setZero(m_robot_util->m_nbJoints); |
163 |
|
|
m_current_des.setZero(m_robot_util->m_nbJoints); |
164 |
|
|
m_tauErrIntegral.setZero(m_robot_util->m_nbJoints); |
165 |
|
|
// m_dqDesIntegral.setZero(m_robot_util->m_nbJoints); |
166 |
|
|
m_dqErrIntegral.setZero(m_robot_util->m_nbJoints); |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
void JointTorqueController::reset_integral() { |
170 |
|
|
m_tauErrIntegral.setZero(); |
171 |
|
|
m_dqErrIntegral.setZero(); |
172 |
|
|
} |
173 |
|
|
|
174 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
175 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
176 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
177 |
|
|
|
178 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) { |
179 |
|
|
// const Eigen::VectorXd& q = m_jointsPositionsSIN(iter); |
180 |
|
|
const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter); |
181 |
|
|
const Eigen::VectorXd& ddq = m_jointsAccelerationsSIN(iter); |
182 |
|
|
const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter); |
183 |
|
|
const Eigen::VectorXd& dtau = m_jointsTorquesDerivativeSIN(iter); |
184 |
|
|
const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter); |
185 |
|
|
// const Eigen::VectorXd& dtau_d = |
186 |
|
|
// m_jointsTorquesDesiredDerivativeSIN(iter); |
187 |
|
|
const Eigen::VectorXd& dq_des = m_dq_desSIN(iter); |
188 |
|
|
const Eigen::VectorXd& kp = m_KpTorqueSIN(iter); |
189 |
|
|
const Eigen::VectorXd& kd = m_KdTorqueSIN(iter); |
190 |
|
|
const Eigen::VectorXd& kd_vel = m_KdVelSIN(iter); |
191 |
|
|
const Eigen::VectorXd& ki_vel = m_KiVelSIN(iter); |
192 |
|
|
const Eigen::VectorXd& tauErrInt = m_torque_error_integralSOUT(iter); |
193 |
|
|
const Eigen::VectorXd& colFricCompPerc = |
194 |
|
|
m_coulomb_friction_compensation_percentageSIN(iter); |
195 |
|
|
const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter); |
196 |
|
|
const Eigen::VectorXd& motorParameterKt_n = m_motorParameterKt_nSIN(iter); |
197 |
|
|
const Eigen::VectorXd& motorParameterKf_p = m_motorParameterKf_pSIN(iter); |
198 |
|
|
const Eigen::VectorXd& motorParameterKf_n = m_motorParameterKf_nSIN(iter); |
199 |
|
|
const Eigen::VectorXd& motorParameterKv_p = m_motorParameterKv_pSIN(iter); |
200 |
|
|
const Eigen::VectorXd& motorParameterKv_n = m_motorParameterKv_nSIN(iter); |
201 |
|
|
const Eigen::VectorXd& motorParameterKa_p = m_motorParameterKa_pSIN(iter); |
202 |
|
|
const Eigen::VectorXd& motorParameterKa_n = m_motorParameterKa_nSIN(iter); |
203 |
|
|
const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter); |
204 |
|
|
// const Eigen::VectorXd& dq_thr = m_dq_thresholdSIN(iter); |
205 |
|
|
|
206 |
|
|
m_tau_star = |
207 |
|
|
tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau); |
208 |
|
|
|
209 |
|
|
int offset = 0; |
210 |
|
|
if (dq.size() == (int)(m_robot_util->m_nbJoints + 6)) offset = 6; |
211 |
|
|
|
212 |
|
|
m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des - dq); |
213 |
|
|
const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter); |
214 |
|
|
// saturate |
215 |
|
|
bool saturating = false; |
216 |
|
|
for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) { |
217 |
|
|
if (m_dqErrIntegral(i) > err_int_sat(i)) { |
218 |
|
|
saturating = true; |
219 |
|
|
m_dqErrIntegral(i) = err_int_sat(i); |
220 |
|
|
} else if (m_dqErrIntegral(i) < -err_int_sat(i)) { |
221 |
|
|
saturating = true; |
222 |
|
|
m_dqErrIntegral(i) = -err_int_sat(i); |
223 |
|
|
} |
224 |
|
|
} |
225 |
|
|
if (saturating) |
226 |
|
|
SEND_INFO_STREAM_MSG("Saturate dqErr integral: " + |
227 |
|
|
toString(m_dqErrIntegral.head<12>())); |
228 |
|
|
|
229 |
|
|
for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) { |
230 |
|
|
m_current_des(i) = motorModel.getCurrent( |
231 |
|
|
m_tau_star(i), |
232 |
|
|
dq(i + offset) + kd_vel(i) * (dq_des(i) - dq(i + offset)) + |
233 |
|
|
m_dqErrIntegral(i), // ki_vel(i)*(m_dqDesIntegral(i)-q(i)), |
234 |
|
|
ddq(i + offset), motorParameterKt_p(i), motorParameterKt_n(i), |
235 |
|
|
motorParameterKf_p(i) * colFricCompPerc(i), |
236 |
|
|
motorParameterKf_n(i) * colFricCompPerc(i), motorParameterKv_p(i), |
237 |
|
|
motorParameterKv_n(i), motorParameterKa_p(i), motorParameterKa_n(i), |
238 |
|
|
static_cast<unsigned int>(polySignDq(i))); |
239 |
|
|
} |
240 |
|
|
|
241 |
|
|
s = m_current_des; |
242 |
|
|
return s; |
243 |
|
|
} |
244 |
|
|
|
245 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(torque_error_integral, dynamicgraph::Vector) { |
246 |
|
|
const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter); |
247 |
|
|
const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter); |
248 |
|
|
const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter); |
249 |
|
|
const Eigen::VectorXd& ki = m_KiTorqueSIN(iter); |
250 |
|
|
|
251 |
|
|
// compute torque error integral and saturate |
252 |
|
|
m_tauErrIntegral += m_dt * ki.cwiseProduct(tau_d - tau); |
253 |
|
|
for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) { |
254 |
|
|
if (m_tauErrIntegral(i) > err_int_sat(i)) |
255 |
|
|
m_tauErrIntegral(i) = err_int_sat(i); |
256 |
|
|
else if (m_tauErrIntegral(i) < -err_int_sat(i)) |
257 |
|
|
m_tauErrIntegral(i) = -err_int_sat(i); |
258 |
|
|
} |
259 |
|
|
|
260 |
|
|
s = m_tauErrIntegral; |
261 |
|
|
return s; |
262 |
|
|
} |
263 |
|
|
|
264 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(smoothSignDq, dynamicgraph::Vector) { |
265 |
|
|
const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter); |
266 |
|
|
const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter); |
267 |
|
|
if (s.size() != (int)m_robot_util->m_nbJoints) |
268 |
|
|
s.resize(m_robot_util->m_nbJoints); |
269 |
|
|
|
270 |
|
|
for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) |
271 |
|
|
s(i) = motorModel.smoothSign(dq[i], 0.1, |
272 |
|
|
static_cast<unsigned int>(polySignDq[i])); |
273 |
|
|
// TODO Use Eigen binaryexpr |
274 |
|
|
return s; |
275 |
|
|
} |
276 |
|
|
|
277 |
|
|
void JointTorqueController::display(std::ostream& os) const { |
278 |
|
|
os << "JointTorqueController " << getName() << ":\n"; |
279 |
|
|
try { |
280 |
|
|
getProfiler().report_all(3, os); |
281 |
|
|
} catch (ExceptionSignal e) { |
282 |
|
|
} |
283 |
|
|
} |
284 |
|
|
|
285 |
|
|
} // namespace torque_control |
286 |
|
|
} // namespace sot |
287 |
|
|
} // namespace dynamicgraph |