sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
control-manager.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #include <tsid/robots/robot-wrapper.hpp>
7 #include <tsid/utils/stop-watch.hpp>
8 #include <tsid/utils/statistics.hpp>
9 
10 #include <dynamic-graph/factory.h>
11 #include <sot/core/debug.hh>
14 
15 using namespace tsid;
16 
17 namespace dynamicgraph {
18 namespace sot {
19 namespace torque_control {
20 namespace dynamicgraph = ::dynamicgraph;
21 using namespace dynamicgraph;
22 using namespace dynamicgraph::command;
23 using namespace std;
24 using namespace dg::sot::torque_control;
25 
26 // Size to be aligned "-------------------------------------------------------"
27 #define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
28 #define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
29 
30 #define INPUT_SIGNALS m_i_maxSIN << m_u_maxSIN << m_tau_maxSIN << m_tauSIN << m_tau_predictedSIN << m_i_measuredSIN
31 #define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT
32 
35 typedef ControlManager EntityClassName;
36 
37 /* --- DG FACTORY ---------------------------------------------------- */
39 
40 /* ------------------------------------------------------------------- */
41 /* --- CONSTRUCTION -------------------------------------------------- */
42 /* ------------------------------------------------------------------- */
43 ControlManager::ControlManager(const std::string& name)
44  : Entity(name),
45  CONSTRUCT_SIGNAL_IN(i_measured, dynamicgraph::Vector),
46  CONSTRUCT_SIGNAL_IN(tau, dynamicgraph::Vector),
47  CONSTRUCT_SIGNAL_IN(tau_predicted, dynamicgraph::Vector),
48  CONSTRUCT_SIGNAL_IN(i_max, dynamicgraph::Vector),
49  CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector),
50  CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector),
51  CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_i_measuredSIN),
52  CONSTRUCT_SIGNAL_OUT(u_safe, dynamicgraph::Vector, INPUT_SIGNALS << m_uSOUT),
53  m_robot_util(RefVoidRobotUtil()),
54  m_initSucceeded(false),
55  m_emergency_stop_triggered(false),
56  m_is_first_iter(true),
57  m_iter(0),
58  m_sleep_time(0.0) {
59  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
60 
61  /* Commands. */
62  addCommand("init", makeCommandVoid3(*this, &ControlManager::init,
63  docCommandVoid3("Initialize the entity.", "Time period in seconds (double)",
64  "URDF file path (string)", "Robot reference (string)")));
65  addCommand(
66  "addCtrlMode",
67  makeCommandVoid1(*this, &ControlManager::addCtrlMode,
68  docCommandVoid1("Create an input signal with name 'ctrl_x' where x is the specified name.",
69  "Name (string)")));
70 
71  addCommand("ctrlModes", makeCommandVoid0(*this, &ControlManager::ctrlModes,
72  docCommandVoid0("Get a list of all the available control modes.")));
73 
74  addCommand("setCtrlMode", makeCommandVoid2(*this, &ControlManager::setCtrlMode,
75  docCommandVoid2("Set the control mode for a joint.",
76  "(string) joint name", "(string) control mode")));
77 
78  addCommand("getCtrlMode",
79  makeCommandVoid1(*this, &ControlManager::getCtrlMode,
80  docCommandVoid1("Get the control mode of a joint.", "(string) joint name")));
81 
82  addCommand("resetProfiler",
83  makeCommandVoid0(
85  docCommandVoid0("Reset the statistics computed by the profiler (print this entity to see them).")));
86 
87  addCommand("setNameToId", makeCommandVoid2(*this, &ControlManager::setNameToId,
88  docCommandVoid2("Set map for a name to an Id", "(string) joint name",
89  "(double) joint id")));
90 
91  addCommand("setForceNameToForceId",
92  makeCommandVoid2(*this, &ControlManager::setForceNameToForceId,
93  docCommandVoid2("Set map from a force sensor name to a force sensor Id",
94  "(string) force sensor name", "(double) force sensor id")));
95 
96  addCommand("setJointLimitsFromId",
97  makeCommandVoid3(*this, &ControlManager::setJointLimitsFromId,
98  docCommandVoid3("Set the joint limits for a given joint ID", "(double) joint id",
99  "(double) lower limit", "(double) uppper limit")));
100 
101  addCommand(
102  "setForceLimitsFromId",
103  makeCommandVoid3(*this, &ControlManager::setForceLimitsFromId,
104  docCommandVoid3("Set the force limits for a given force sensor ID", "(double) force sensor id",
105  "(double) lower limit", "(double) uppper limit")));
106 
107  addCommand("setJointsUrdfToSot",
108  makeCommandVoid1(*this, &ControlManager::setJoints,
109  docCommandVoid1("Map Joints From URDF to SoT.", "Vector of integer for mapping")));
110 
111  addCommand("setRightFootSoleXYZ",
112  makeCommandVoid1(*this, &ControlManager::setRightFootSoleXYZ,
113  docCommandVoid1("Set the right foot sole 3d position.", "Vector of 3 doubles")));
114  addCommand("setRightFootForceSensorXYZ",
115  makeCommandVoid1(*this, &ControlManager::setRightFootForceSensorXYZ,
116  docCommandVoid1("Set the right foot sensor 3d position.", "Vector of 3 doubles")));
117 
118  addCommand("setFootFrameName", makeCommandVoid2(*this, &ControlManager::setFootFrameName,
119  docCommandVoid2("Set the Frame Name for the Foot Name.",
120  "(string) Foot name", "(string) Frame name")));
121  addCommand("setHandFrameName", makeCommandVoid2(*this, &ControlManager::setHandFrameName,
122  docCommandVoid2("Set the Frame Name for the Hand Name.",
123  "(string) Hand name", "(string) Frame name")));
124  addCommand("setImuJointName",
125  makeCommandVoid1(*this, &ControlManager::setImuJointName,
126  docCommandVoid1("Set the Joint Name wich IMU is attached to.", "(string) Joint name")));
127  addCommand("displayRobotUtil", makeCommandVoid0(*this, &ControlManager::displayRobotUtil,
128  docCommandVoid0("Display the current robot util data set.")));
129 
130  addCommand("setStreamPrintPeriod", makeCommandVoid1(*this, &ControlManager::setStreamPrintPeriod,
131  docCommandVoid1("Set the period used for printing in streaming.",
132  "Print period in seconds (double)")));
133 
134  addCommand("setSleepTime",
135  makeCommandVoid1(*this, &ControlManager::setSleepTime,
136  docCommandVoid1("Set the time to sleep at every iteration (to slow down simulation).",
137  "Sleep time in seconds (double)")));
138 
139  addCommand(
140  "addEmergencyStopSIN",
141  makeCommandVoid1(
143  docCommandVoid1("Add emergency signal input from another entity that can stop the control if necessary.",
144  "(string) signal name : 'emergencyStop_' + name")));
145 }
146 
147 void ControlManager::init(const double& dt, const std::string& urdfFile, const std::string& robotRef) {
148  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
149  m_dt = dt;
151  m_initSucceeded = true;
152  vector<string> package_dirs;
153  m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
154  std::string localName(robotRef);
155  if (!isNameInRobotUtil(localName)) {
156  m_robot_util = createRobotUtil(localName);
157  SEND_MSG("createRobotUtil success\n", MSG_TYPE_INFO);
158  } else {
159  m_robot_util = getRobotUtil(localName);
160  SEND_MSG("getRobotUtil success\n", MSG_TYPE_INFO);
161  }
162  SEND_MSG(m_robot_util->m_urdf_filename, MSG_TYPE_INFO);
163  m_robot_util->m_urdf_filename = urdfFile;
164  addCommand("getJointsUrdfToSot", makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
165  docDirectSetter("Display map Joints From URDF to SoT.",
166  "Vector of integer for mapping")));
167 
168  m_robot_util->m_nbJoints = m_robot->nv() - 6;
169  m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints);
170  m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints);
171  m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints, 0);
172 }
173 
174 /* ------------------------------------------------------------------- */
175 /* --- SIGNALS ------------------------------------------------------- */
176 /* ------------------------------------------------------------------- */
177 
178 DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
179  if (!m_initSucceeded) {
180  SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
181  return s;
182  }
183 
184  if (m_is_first_iter)
185  m_is_first_iter = false;
186  else
187  getProfiler().stop(PROFILE_DYNAMIC_GRAPH_PERIOD);
188  getProfiler().start(PROFILE_DYNAMIC_GRAPH_PERIOD);
189 
190  if (s.size() != (Eigen::VectorXd::Index)m_robot_util->m_nbJoints) s.resize(m_robot_util->m_nbJoints);
191 
192  getProfiler().start(PROFILE_PWM_DESIRED_COMPUTATION);
193  {
194  // trigger computation of all ctrl inputs
195  for (unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++) (*m_ctrlInputsSIN[i])(iter);
196 
197  int cm_id, cm_id_prev;
198  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
199  cm_id = m_jointCtrlModes_current[i].id;
200  if (cm_id < 0) {
201  SEND_MSG("You forgot to set the control mode of joint " + toString(i), MSG_TYPE_ERROR_STREAM);
202  continue;
203  }
204 
205  const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
206  assert(ctrl.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
207 
208  if (m_jointCtrlModesCountDown[i] == 0)
209  s(i) = ctrl(i);
210  else {
211  cm_id_prev = m_jointCtrlModes_previous[i].id;
212  const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
213  assert(ctrl_prev.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
214 
215  double alpha = m_jointCtrlModesCountDown[i] / CTRL_MODE_TRANSITION_TIME_STEP;
216  // SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
217  // " to "+toString(cm_id)+" alpha="+toString(alpha),MSG_TYPE_DEBUG);
218  s(i) = alpha * ctrl_prev(i) + (1 - alpha) * ctrl(i);
219  m_jointCtrlModesCountDown[i]--;
220 
221  if (m_jointCtrlModesCountDown[i] == 0) {
222  SEND_MSG(
223  "Joint " + toString(i) + " changed ctrl mode from " + toString(cm_id_prev) + " to " + toString(cm_id),
224  MSG_TYPE_INFO);
225  updateJointCtrlModesOutputSignal();
226  }
227  }
228  }
229  }
230  getProfiler().stop(PROFILE_PWM_DESIRED_COMPUTATION);
231 
232  usleep(static_cast<unsigned int>(1e6 * m_sleep_time));
233  if (m_sleep_time >= 0.1) {
234  for (unsigned int i = 0; i < m_ctrlInputsSIN.size(); i++) {
235  const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
236  SEND_MSG(toString(iter) + ") tau =" + toString(ctrl, 1, 4, " ") + m_ctrlModes[i], MSG_TYPE_ERROR);
237  }
238  }
239 
240  return s;
241 }
242 
243 DEFINE_SIGNAL_OUT_FUNCTION(u_safe, dynamicgraph::Vector) {
244  if (!m_initSucceeded) {
245  SEND_WARNING_STREAM_MSG("Cannot compute signal u_safe before initialization!");
246  return s;
247  }
248 
249  const dynamicgraph::Vector& u = m_uSOUT(iter);
250  const dynamicgraph::Vector& tau_max = m_tau_maxSIN(iter);
251  const dynamicgraph::Vector& ctrl_max = m_u_maxSIN(iter);
252  const dynamicgraph::Vector& i_max = m_i_maxSIN(iter);
253  const dynamicgraph::Vector& tau = m_tauSIN(iter);
254  const dynamicgraph::Vector& i_real = m_i_measuredSIN(iter);
255  const dynamicgraph::Vector& tau_predicted = m_tau_predictedSIN(iter);
256 
257  for (std::size_t i = 0; i < m_emergencyStopSIN.size(); i++) {
258  if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter)) {
259  m_emergency_stop_triggered = true;
260  SEND_MSG("Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
261  }
262  }
263 
264  s = u;
265 
266  if (!m_emergency_stop_triggered) {
267  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
268  if (fabs(tau(i)) > tau_max(i)) {
269  m_emergency_stop_triggered = true;
270  SEND_MSG("Estimated torque " + toString(tau(i)) + " > max torque " + toString(tau_max(i)) + " for joint " +
271  m_robot_util->get_name_from_id(i),
272  MSG_TYPE_ERROR);
273  }
274 
275  if (fabs(tau_predicted(i)) > tau_max(i)) {
276  m_emergency_stop_triggered = true;
277  SEND_MSG("Predicted torque " + toString(tau_predicted(i)) + " > max torque " + toString(tau_max(i)) +
278  " for joint " + m_robot_util->get_name_from_id(i),
279  MSG_TYPE_ERROR);
280  }
281 
282  if (fabs(i_real(i)) > i_max(i)) {
283  m_emergency_stop_triggered = true;
284  SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
285  " measured current is too large: " + toString(i_real(i)) + "A > " + toString(i_max(i)) + "A",
286  MSG_TYPE_ERROR);
287  break;
288  }
289 
290  if (fabs(u(i)) > ctrl_max(i)) {
291  m_emergency_stop_triggered = true;
292  SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) + " desired current is too large: " + toString(u(i)) +
293  "A > " + toString(ctrl_max(i)) + "A",
294  MSG_TYPE_ERROR);
295  break;
296  }
297  }
298  }
299 
300  if (m_emergency_stop_triggered) s.setZero();
301 
302  return s;
303 }
304 
305 /* --- COMMANDS ---------------------------------------------------------- */
306 
307 void ControlManager::addCtrlMode(const string& name) {
308  // check there is no other control mode with the same name
309  for (unsigned int i = 0; i < m_ctrlModes.size(); i++)
310  if (name == m_ctrlModes[i]) return SEND_MSG("It already exists a control mode with name " + name, MSG_TYPE_ERROR);
311 
312  // create a new input signal to read the new control
313  m_ctrlInputsSIN.push_back(new SignalPtr<dynamicgraph::Vector, int>(
314  NULL, getClassName() + "(" + getName() + ")::input(dynamicgraph::Vector)::ctrl_" + name));
315 
316  // create a new output signal to specify which joints are controlled with the new
317  // control mode
318  m_jointsCtrlModesSOUT.push_back(new Signal<dynamicgraph::Vector, int>(
319  getClassName() + "(" + getName() + ")::output(dynamicgraph::Vector)::joints_ctrl_mode_" + name));
320 
321  // add the new control mode to the list of available control modes
322  m_ctrlModes.push_back(name);
323 
324  // register the new signals and add the new signal dependecy
325  Eigen::VectorXd::Index i = m_ctrlModes.size() - 1;
326  m_uSOUT.addDependency(*m_ctrlInputsSIN[i]);
327  Entity::signalRegistration(*m_ctrlInputsSIN[i]);
328  Entity::signalRegistration(*m_jointsCtrlModesSOUT[i]);
330 }
331 
332 void ControlManager::ctrlModes() { SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO); }
333 
334 void ControlManager::setCtrlMode(const string& jointName, const string& ctrlMode) {
335  CtrlMode cm;
336  if (convertStringToCtrlMode(ctrlMode, cm) == false) return;
337 
338  if (jointName == "all") {
339  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) setCtrlMode(i, cm);
340  } else {
341  // decompose strings like "rk-rhp-lhp-..."
342  std::stringstream ss(jointName);
343  std::string item;
344  std::list<int> jIdList;
345  unsigned int i;
346  while (std::getline(ss, item, '-')) {
347  SEND_MSG("parsed joint : " + item, MSG_TYPE_INFO);
348  if (convertJointNameToJointId(item, i)) jIdList.push_back(i);
349  }
350  for (std::list<int>::iterator it = jIdList.begin(); it != jIdList.end(); ++it) setCtrlMode(*it, cm);
351  }
353 }
354 
355 void ControlManager::setCtrlMode(const int jid, const CtrlMode& cm) {
356  if (m_jointCtrlModesCountDown[jid] != 0)
357  return SEND_MSG("Cannot change control mode of joint " + m_robot_util->get_name_from_id(jid) +
358  " because its previous ctrl mode transition has not terminated yet: " +
359  toString(m_jointCtrlModesCountDown[jid]),
360  MSG_TYPE_ERROR);
361 
362  if (cm.id == m_jointCtrlModes_current[jid].id)
363  return SEND_MSG("Cannot change control mode of joint " + m_robot_util->get_name_from_id(jid) +
364  " because it has already the specified ctrl mode",
365  MSG_TYPE_ERROR);
366 
367  if (m_jointCtrlModes_current[jid].id < 0) {
368  // first setting of the control mode
369  m_jointCtrlModes_previous[jid] = cm;
370  m_jointCtrlModes_current[jid] = cm;
371  } else {
374  m_jointCtrlModes_current[jid] = cm;
375  }
376 }
377 
378 void ControlManager::getCtrlMode(const std::string& jointName) {
379  if (jointName == "all") {
380  stringstream ss;
381  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
382  ss << m_robot_util->get_name_from_id(i) << " " << m_jointCtrlModes_current[i] << "; ";
383  SEND_MSG(ss.str(), MSG_TYPE_INFO);
384  return;
385  }
386 
387  unsigned int i;
388  if (convertJointNameToJointId(jointName, i) == false) return;
389  SEND_MSG("The control mode of joint " + jointName + " is " + m_jointCtrlModes_current[i].name, MSG_TYPE_INFO);
390 }
391 
393  getProfiler().reset_all();
394  getStatistics().reset_all();
395 }
396 
398 
399 void ControlManager::setSleepTime(const double& seconds) {
400  if (seconds < 0.0) return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
401  m_sleep_time = seconds;
402 }
403 
404 void ControlManager::addEmergencyStopSIN(const string& name) {
405  SEND_MSG("New emergency signal input emergencyStop_" + name + " created", MSG_TYPE_INFO);
406  // create a new input signal
407  m_emergencyStopSIN.push_back(
408  new SignalPtr<bool, int>(NULL, getClassName() + "(" + getName() + ")::input(bool)::emergencyStop_" + name));
409 
410  // register the new signals and add the new signal dependecy
411  Eigen::VectorXd::Index i = m_emergencyStopSIN.size() - 1;
412  m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
413  Entity::signalRegistration(*m_emergencyStopSIN[i]);
414 }
415 
416 void ControlManager::setNameToId(const std::string& jointName, const double& jointId) {
417  if (!m_initSucceeded) {
418  SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
419  return;
420  }
421  m_robot_util->set_name_to_id(jointName, static_cast<Eigen::VectorXd::Index>(jointId));
422 }
423 
424 void ControlManager::setJointLimitsFromId(const double& jointId, const double& lq, const double& uq) {
425  if (!m_initSucceeded) {
426  SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id before initialization!");
427  return;
428  }
429 
430  m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
431 }
432 
433 void ControlManager::setForceLimitsFromId(const double& jointId, const dynamicgraph::Vector& lq,
434  const dynamicgraph::Vector& uq) {
435  if (!m_initSucceeded) {
436  SEND_WARNING_STREAM_MSG("Cannot set force limits from force id before initialization!");
437  return;
438  }
439 
440  m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
441 }
442 
443 void ControlManager::setForceNameToForceId(const std::string& forceName, const double& forceId) {
444  if (!m_initSucceeded) {
445  SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id before initialization!");
446  return;
447  }
448 
449  m_robot_util->m_force_util.set_name_to_force_id(forceName, static_cast<Eigen::VectorXd::Index>(forceId));
450 }
451 
452 void ControlManager::setJoints(const dg::Vector& urdf_to_sot) {
453  if (!m_initSucceeded) {
454  SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
455  return;
456  }
457  m_robot_util->set_urdf_to_sot(urdf_to_sot);
458 }
459 
460 void ControlManager::setRightFootSoleXYZ(const dynamicgraph::Vector& xyz) {
461  if (!m_initSucceeded) {
462  SEND_WARNING_STREAM_MSG("Cannot set right foot sole XYZ before initialization!");
463  return;
464  }
465 
466  m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
467 }
468 
469 void ControlManager::setRightFootForceSensorXYZ(const dynamicgraph::Vector& xyz) {
470  if (!m_initSucceeded) {
471  SEND_WARNING_STREAM_MSG("Cannot set right foot force sensor XYZ before initialization!");
472  return;
473  }
474 
475  m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
476 }
477 
478 void ControlManager::setFootFrameName(const std::string& FootName, const std::string& FrameName) {
479  if (!m_initSucceeded) {
480  SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
481  return;
482  }
483  if (FootName == "Left")
484  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
485  else if (FootName == "Right")
486  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
487  else
488  SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
489 }
490 
491 void ControlManager::setHandFrameName(const std::string& HandName, const std::string& FrameName) {
492  if (!m_initSucceeded) {
493  SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
494  return;
495  }
496  if (HandName == "Left")
497  m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
498  else if (HandName == "Right")
499  m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
500  else
501  SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
502 }
503 
504 void ControlManager::setImuJointName(const std::string& JointName) {
505  if (!m_initSucceeded) {
506  SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
507  return;
508  }
509  m_robot_util->m_imu_joint_name = JointName;
510 }
511 
512 void ControlManager::displayRobotUtil() { m_robot_util->display(std::cout); }
513 
514 /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
515 
517  if (m_robot_util->m_nbJoints == 0) {
518  SEND_MSG("You should call init first. The size of the vector is unknown.", MSG_TYPE_ERROR);
519  return;
520  }
521 
522  dynamicgraph::Vector cm(m_robot_util->m_nbJoints);
523  for (unsigned int i = 0; i < m_jointsCtrlModesSOUT.size(); i++) {
524  for (unsigned int j = 0; j < m_robot_util->m_nbJoints; j++) {
525  cm(j) = 0;
526  if ((unsigned int)m_jointCtrlModes_current[j].id == i) cm(j) = 1;
527 
528  // during the transition between two ctrl modes they both result active
529  if (m_jointCtrlModesCountDown[j] > 0 && (unsigned int)m_jointCtrlModes_previous[j].id == i) cm(j) = 1;
530  }
531  m_jointsCtrlModesSOUT[i]->setConstant(cm);
532  }
533 }
534 
535 bool ControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm) {
536  // Check if the ctrl mode name exists
537  for (unsigned int i = 0; i < m_ctrlModes.size(); i++)
538  if (name == m_ctrlModes[i]) {
539  cm = CtrlMode(i, name);
540  return true;
541  }
542  SEND_MSG("The specified control mode does not exist", MSG_TYPE_ERROR);
543  SEND_MSG("Possible control modes are: " + toString(m_ctrlModes), MSG_TYPE_INFO);
544  return false;
545 }
546 
547 bool ControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id) {
548  // Check if the joint name exists
549  sot::Index jid = m_robot_util->get_id_from_name(name);
550  if (jid < 0) {
551  SEND_MSG("The specified joint name does not exist: " + name, MSG_TYPE_ERROR);
552  std::stringstream ss;
553  for (pinocchio::Model::JointIndex it = 0; it < m_robot_util->m_nbJoints; it++)
554  ss << m_robot_util->get_name_from_id(it) << ", ";
555  SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
556  return false;
557  }
558  id = (unsigned int)jid;
559  return true;
560 }
561 
562 bool ControlManager::isJointInRange(unsigned int id, double q) {
563  const JointLimits& JL = m_robot_util->get_joint_limits_from_id((Index)id);
564 
565  double jl = JL.lower;
566  if (q < jl) {
567  SEND_MSG("Desired joint angle " + toString(q) + " is smaller than lower limit: " + toString(jl), MSG_TYPE_ERROR);
568  return false;
569  }
570  double ju = JL.upper;
571  if (q > ju) {
572  SEND_MSG("Desired joint angle " + toString(q) + " is larger than upper limit: " + toString(ju), MSG_TYPE_ERROR);
573  return false;
574  }
575  return true;
576 }
577 
578 /* ------------------------------------------------------------------- */
579 /* --- ENTITY -------------------------------------------------------- */
580 /* ------------------------------------------------------------------- */
581 
582 void ControlManager::display(std::ostream& os) const {
583  os << "ControlManager " << getName();
584  try {
585  getProfiler().report_all(3, os);
586  } catch (ExceptionSignal e) {
587  }
588 }
589 
590 } // namespace torque_control
591 } // namespace sot
592 } // namespace dynamicgraph
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
bool convertStringToCtrlMode(const std::string &name, CtrlMode &cm)
counters used for the transition between two ctrl modes
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &)
double m_dt
true if the entity has been successfully initialized
void setCtrlMode(const std::string &jointName, const std::string &ctrlMode)
virtual void display(std::ostream &os) const
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
void setForceLimitsFromId(const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq)
Command related to ForceUtil.
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
std::vector< dynamicgraph::SignalPtr< bool, int > * > m_emergencyStopSIN
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
void init(const double &dt, const std::string &urdfFile, const std::string &robotRef)
void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq)
void setForceNameToForceId(const std::string &forceName, const double &forceId)
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
bool m_emergency_stop_triggered
control loop time period
void addCtrlMode(const std::string &name)
same as u when everything is fine, 0 otherwise
void setRightFootSoleXYZ(const dynamicgraph::Vector &)
Commands related to FootUtil.
void setNameToId(const std::string &jointName, const double &jointId)
Commands related to joint name and joint id.
void setJoints(const dynamicgraph::Vector &)
Set the mapping between urdf and sot.
void getCtrlMode(const std::string &jointName)
void setHandFrameName(const std::string &, const std::string &)
Commands related to HandUtil.
void setFootFrameName(const std::string &, const std::string &)
#define PROFILE_PWM_DESIRED_COMPUTATION
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
#define PROFILE_DYNAMIC_GRAPH_PERIOD
#define CTRL_MODE_TRANSITION_TIME_STEP
Number of time step to transition from one ctrl mode to another.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceTorqueCtrl, "DeviceTorqueCtrl")
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
to read text file
Definition: treeview.dox:22