6 #include <tsid/utils/stop-watch.hpp>
7 #include <tsid/utils/statistics.hpp>
9 #include <dynamic-graph/factory.h>
10 #include <sot/core/debug.hh>
21 using namespace dg::sot::torque_control;
23 #define SAFETY_SIGNALS m_i_maxSIN << m_u_maxSIN << m_u_saturationSIN
24 #define INPUT_SIGNALS \
25 m_i_desSIN << m_percentage_dead_zone_compensationSIN << SAFETY_SIGNALS << m_i_max_dead_zone_compensationSIN \
26 << m_dqSIN << m_bemf_factorSIN << m_in_out_gainSIN << m_i_measuredSIN << m_dead_zone_offsetsSIN \
27 << m_i_sens_gainsSIN << m_i_sensor_offsets_low_levelSIN << m_i_sensor_offsets_real_inSIN \
28 << m_kp_currentSIN << m_ki_currentSIN << m_percentage_bemf_compensationSIN
29 #define OUTPUT_SIGNALS \
30 m_uSOUT << m_u_safeSOUT << m_i_realSOUT << m_i_low_levelSOUT << m_dead_zone_compensationSOUT << m_i_errorsSOUT \
31 << m_i_errors_ll_wo_bemfSOUT << m_i_sensor_offsets_real_outSOUT
57 CONSTRUCT_SIGNAL_IN(percentage_bemf_compensation,
dynamicgraph::Vector),
58 CONSTRUCT_SIGNAL_IN(dead_zone_offsets,
dynamicgraph::Vector),
59 CONSTRUCT_SIGNAL_IN(percentage_dead_zone_compensation,
dynamicgraph::Vector),
60 CONSTRUCT_SIGNAL_IN(i_max_dead_zone_compensation,
dynamicgraph::Vector),
61 CONSTRUCT_SIGNAL_IN(i_sensor_offsets_low_level,
dynamicgraph::Vector),
62 CONSTRUCT_SIGNAL_IN(i_sensor_offsets_real_in,
dynamicgraph::Vector),
63 CONSTRUCT_SIGNAL_OUT(u,
dynamicgraph::Vector, m_i_desSIN),
66 INPUT_SIGNALS << m_uSOUT << m_i_realSOUT << m_i_low_levelSOUT << m_i_sensor_offsets_real_outSOUT),
68 m_i_measuredSIN << m_i_sens_gainsSIN << m_i_sensor_offsets_real_outSOUT),
70 m_i_measuredSIN << m_i_sens_gainsSIN << m_i_sensor_offsets_low_levelSIN),
71 CONSTRUCT_SIGNAL_OUT(i_sensor_offsets_real_out,
dynamicgraph::Vector,
72 m_i_measuredSIN << m_i_sensor_offsets_real_inSIN),
73 CONSTRUCT_SIGNAL_OUT(dead_zone_compensation,
dynamicgraph::Vector, m_u_safeSOUT << m_dead_zone_offsetsSIN),
74 CONSTRUCT_SIGNAL_OUT(i_errors,
dynamicgraph::Vector, m_i_realSOUT << m_uSOUT),
77 m_i_realSOUT << m_uSOUT << m_percentage_bemf_compensationSIN << m_dqSIN << m_bemf_factorSIN),
78 m_robot_util(RefVoidRobotUtil()),
79 m_initSucceeded(false),
80 m_emergency_stop_triggered(false),
81 m_is_first_iter(true),
86 addCommand(
"init", makeCommandVoid3(
89 "Initialize the entity.",
"Time period in seconds (double)",
"Robot reference (string)",
90 "Number of iterations while control is disabled to calibrate current sensors (int)")));
93 docCommandVoid0(
"Reset the integral error.")));
97 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
102 std::string localName(robotRef);
103 if (!isNameInRobotUtil(localName)) {
121 if (!m_initSucceeded) {
122 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u before initialization!");
126 if (m_is_first_iter) m_is_first_iter =
false;
128 if (s.size() != (Eigen::VectorXd::Index)m_robot_util->m_nbJoints) s.resize(m_robot_util->m_nbJoints);
130 const dynamicgraph::Vector& i_des = m_i_desSIN(iter);
131 const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
132 const dynamicgraph::Vector& i_ll = m_i_low_levelSOUT(iter);
133 const dynamicgraph::Vector& cur_sens_gains = m_i_sens_gainsSIN(iter);
134 const dynamicgraph::Vector& i_offset_real = m_i_sensor_offsets_real_outSOUT(iter);
135 const dynamicgraph::Vector& dq = m_dqSIN(iter);
137 const dynamicgraph::Vector& dead_zone_offsets = m_dead_zone_offsetsSIN(iter);
138 const dynamicgraph::Vector& dead_zone_comp_perc = m_percentage_dead_zone_compensationSIN(iter);
139 const dynamicgraph::Vector& bemf_factor = m_bemf_factorSIN(iter);
140 const dynamicgraph::Vector& bemf_comp_perc = m_percentage_bemf_compensationSIN(iter);
141 const dynamicgraph::Vector& i_max_dz_comp = m_i_max_dead_zone_compensationSIN(iter);
142 const dynamicgraph::Vector& kp = m_kp_currentSIN(iter);
143 const dynamicgraph::Vector& ki = m_ki_currentSIN(iter);
145 m_i_err_integr += ki.cwiseProduct(i_des - i_real);
147 s = i_des + m_i_err_integr;
148 s += kp.cwiseProduct(i_des - i_real);
149 s += cur_sens_gains.cwiseProduct(i_offset_real);
150 s += bemf_comp_perc.cwiseProduct(bemf_factor.cwiseProduct(dq));
152 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
153 double err = s(i) - i_ll(i);
154 if (err > i_max_dz_comp(i))
156 else if (err < -i_max_dz_comp(i))
157 m_dz_coeff(i) = -1.0;
159 m_dz_coeff(i) = err / i_max_dz_comp(i);
163 s += m_dz_coeff.cwiseProduct(dead_zone_comp_perc.cwiseProduct(dead_zone_offsets));
167 if (m_emergency_stop_triggered || m_iter <
static_cast<int>(m_currentOffsetIters)) s.setZero();
173 if (!m_initSucceeded) {
174 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u_safe before initialization!");
178 const dynamicgraph::Vector& u = m_uSOUT(iter);
179 const dynamicgraph::Vector& u_max = m_u_maxSIN(iter);
180 const dynamicgraph::Vector& u_saturation = m_u_saturationSIN(iter);
181 const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
182 const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
184 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
186 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
187 double i_max = m_i_maxSIN(iter)(i);
188 if ((fabs(i_real(i)) > i_max)) {
189 m_emergency_stop_triggered =
true;
190 SEND_MSG(
"Joint " + m_robot_util->get_name_from_id(i) +
191 " measured current is too large: " + toString(i_real(i)) +
"A > " + toString(i_max) +
"A",
195 if (fabs(u(i)) > u_max(i)) {
196 m_emergency_stop_triggered =
true;
197 SEND_MSG(
"Joint " + m_robot_util->get_name_from_id(i) +
" control is too large: " + toString(u(i)) +
"A > " +
198 toString(u_max(i)) +
"A",
202 s(i) = u(i) * in_out_gain(i);
205 if (s(i) > u_saturation(i))
206 s(i) = u_saturation(i);
207 else if (s(i) < -u_saturation(i))
208 s(i) = -u_saturation(i);
212 if (m_emergency_stop_triggered || m_iter <
static_cast<int>(m_currentOffsetIters)) s.setZero();
218 if (!m_initSucceeded) {
219 SEND_WARNING_STREAM_MSG(
"Cannot compute signal i_real before initialization!");
222 s = m_i_measuredSIN(iter);
225 const dynamicgraph::Vector& offset = m_i_sensor_offsets_real_outSOUT(iter);
229 const dynamicgraph::Vector& K = m_i_sens_gainsSIN(iter);
230 s = s.cwiseProduct(K);
236 if (!m_initSucceeded) {
237 SEND_WARNING_STREAM_MSG(
"Cannot compute signal currents_low_level before initialization!");
240 s = m_i_measuredSIN(iter);
242 const dynamicgraph::Vector& i_offsets_low_level = m_i_sensor_offsets_low_levelSIN(iter);
243 s -= i_offsets_low_level;
245 const dynamicgraph::Vector& K = m_i_sens_gainsSIN(iter);
246 s = s.cwiseProduct(K);
251 if (!m_initSucceeded) {
252 SEND_WARNING_STREAM_MSG(
"Cannot compute signal i_sensor_offsets_real_out before initialization!");
255 const dynamicgraph::Vector& currents = m_i_measuredSIN(iter);
258 if (m_currentOffsetIters > 0) {
259 if (m_iter <
static_cast<int>(m_currentOffsetIters))
260 m_i_offsets_real += (currents - m_i_offsets_real) / (m_iter + 1);
261 else if (m_iter ==
static_cast<int>(m_currentOffsetIters)) {
262 SEND_MSG(
"Current sensor offsets computed in " + toString(m_iter) +
" iterations: " + toString(m_i_offsets_real),
264 for (
int i = 0; i < s.size(); i++)
265 if (fabs(m_i_offsets_real(i)) > 0.6) {
266 SEND_MSG(
"Current offset for joint " + m_robot_util->get_name_from_id(i) +
267 " is too large, suggesting that the sensor may be broken: " + toString(m_i_offsets_real(i)),
269 m_i_offsets_real(i) = 0.0;
275 if (m_i_sensor_offsets_real_inSIN.isPlugged())
276 s = m_i_sensor_offsets_real_inSIN(iter);
278 s = m_i_offsets_real;
284 if (!m_initSucceeded) {
285 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dead_zone_compensation before initialization!");
288 const dynamicgraph::Vector& dead_zone_offsets = m_dead_zone_offsetsSIN(iter);
290 s = m_dz_coeff.cwiseProduct(dead_zone_offsets);
295 if (!m_initSucceeded) {
296 SEND_WARNING_STREAM_MSG(
"Cannot compute signal i_errors before initialization!");
299 const dynamicgraph::Vector& u = m_uSOUT(iter);
300 const dynamicgraph::Vector& currents = m_i_realSOUT(iter);
306 if (!m_initSucceeded) {
307 SEND_WARNING_STREAM_MSG(
"Cannot compute signal i_errors_ll_wo_bemf before initialization!");
310 const dynamicgraph::Vector& u = m_uSOUT(iter);
311 const dynamicgraph::Vector& currents = m_i_realSOUT(iter);
312 const dynamicgraph::Vector& bemfFactor = m_bemf_factorSIN(iter);
313 const dynamicgraph::Vector& bemf_comp_perc = m_percentage_bemf_compensationSIN(iter);
314 const dynamicgraph::Vector& dq = m_dqSIN(iter);
317 (dynamicgraph::Vector::Ones(m_robot_util->m_nbJoints) - bemf_comp_perc)
318 .cwiseProduct(bemfFactor.cwiseProduct(dq));
320 for (
int i = 0; i < s.size(); i++)
322 m_avg_i_err_pos(i) += (s(i) - m_avg_i_err_pos(i)) * 1e-3;
324 m_avg_i_err_neg(i) += (s(i) - m_avg_i_err_neg(i)) * 1e-3;
338 os <<
"CurrentController " << getName();
340 getProfiler().report_all(3, os);
341 }
catch (ExceptionSignal e) {