sot-talos-balance  1.7.0
talos-control-manager.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3  *
4  * This file is part of sot-torque-control.
5  * sot-torque-control is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-torque-control is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
18 #include <sot/core/debug.hh>
19 #include <dynamic-graph/factory.h>
20 
21 #include <dynamic-graph/all-commands.h>
22 #include <sot/core/stop-watch.hh>
24 
25 
26 using namespace sot::talos_balance;
27 
28 namespace dynamicgraph
29 {
30  namespace sot
31  {
32  namespace talos_balance
33  {
34  namespace dynamicgraph = ::dynamicgraph;
35  using namespace dynamicgraph;
36  using namespace dynamicgraph::command;
37  using namespace std;
38  using namespace dg::sot::talos_balance;
39 
40 
41 //Size to be aligned "-------------------------------------------------------"
42 #define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
43 #define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
44 
45 #define INPUT_SIGNALS m_u_maxSIN
46 #define OUTPUT_SIGNALS m_uSOUT << m_u_safeSOUT
47 
50  typedef TalosControlManager EntityClassName;
51 
52  /* --- DG FACTORY ---------------------------------------------------- */
53  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager,
54  "TalosControlManager");
55 
56  /* ------------------------------------------------------------------- */
57  /* --- CONSTRUCTION -------------------------------------------------- */
58  /* ------------------------------------------------------------------- */
60  TalosControlManager(const std::string& name)
61  : Entity(name)
62  ,CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector)
63  ,CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_u_maxSIN)
64  ,CONSTRUCT_SIGNAL_OUT(u_safe, dynamicgraph::Vector, m_uSOUT << m_u_maxSIN)
65  ,m_robot_util(RefVoidRobotUtil())
66  ,m_initSucceeded(false)
67  ,m_emergency_stop_triggered(false)
68  ,m_is_first_iter(true)
69  ,m_iter(0)
70  ,m_sleep_time(0.0)
71  {
72  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
73 
74  /* Commands. */
75  addCommand("init",
76  makeCommandVoid2(*this, &TalosControlManager::init,
77  docCommandVoid2("Initialize the entity.",
78  "Time period in seconds (double)",
79  "Robot reference (string)")));
80  addCommand("addCtrlMode",
81  makeCommandVoid1(*this, &TalosControlManager::addCtrlMode,
82  docCommandVoid1("Create an input signal with name 'ctrl_x' where x is the specified name.",
83  "Name (string)")));
84 
85  addCommand("ctrlModes",
86  makeCommandVoid0(*this, &TalosControlManager::ctrlModes,
87  docCommandVoid0("Get a list of all the available control modes.")));
88 
89  addCommand("setCtrlMode",
90  makeCommandVoid2(*this, &TalosControlManager::setCtrlMode,
91  docCommandVoid2("Set the control mode for a joint.",
92  "(string) joint name",
93  "(string) control mode")));
94 
95  addCommand("getCtrlMode",
96  makeCommandVoid1(*this, &TalosControlManager::getCtrlMode,
97  docCommandVoid1("Get the control mode of a joint.",
98  "(string) joint name")));
99 
100  addCommand("resetProfiler",
101  makeCommandVoid0(*this, &TalosControlManager::resetProfiler,
102  docCommandVoid0("Reset the statistics computed by the profiler (print this entity to see them).")));
103 
104  addCommand("addEmergencyStopSIN",
105  makeCommandVoid1(*this, &TalosControlManager::addEmergencyStopSIN,
106  docCommandVoid1("Add emergency signal input from another entity that can stop the control if necessary.",
107  "(string) signal name : 'emergencyStop_' + name")));
108 
109  addCommand("isEmergencyStopTriggered", makeDirectGetter(*this,&m_emergency_stop_triggered, docDirectGetter("Check whether emergency stop is triggered","bool")));
110  }
111 
112  void TalosControlManager::init(const double & dt,
113  const std::string &robotRef)
114  {
115  if(dt<=0.0)
116  return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
117  m_dt = dt;
119  m_initSucceeded = true;
120  vector<string> package_dirs;
121  std::string localName(robotRef);
122  if (!isNameInRobotUtil(localName))
123  {
124  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
125  }
126  else
127  {
128  m_robot_util = getRobotUtil(localName);
129  }
130  m_numDofs = m_robot_util->m_nbJoints + 6;
131 
132  //TalosControlManager::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) 4);
136  }
137 
138 
139  /* ------------------------------------------------------------------- */
140  /* --- SIGNALS ------------------------------------------------------- */
141  /* ------------------------------------------------------------------- */
142 
144  {
145  if(!m_initSucceeded)
146  {
147  SEND_MSG("Cannot compute signal u before initialization!",MSG_TYPE_WARNING);
148  return s;
149  }
150 
151  if(m_is_first_iter)
152  m_is_first_iter = false;
153 
154  if(s.size()!=(Eigen::VectorXd::Index) m_numDofs)
155  s.resize(m_numDofs);
156 
157  {
158  // trigger computation of all ctrl inputs
159  for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
160  (*m_ctrlInputsSIN[i])(iter);
161 
162  int cm_id, cm_id_prev;
163  for(unsigned int i=0; i<m_numDofs; i++)
164  {
165  cm_id = m_jointCtrlModes_current[i].id;
166  if(cm_id<0)
167  {
168  SEND_MSG("You forgot to set the control mode of joint "+toString(i), MSG_TYPE_ERROR_STREAM);
169  continue;
170  }
171 
172  const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
173  assert(ctrl.size()==m_numDofs);
174 
175  if(m_jointCtrlModesCountDown[i]==0)
176  s(i) = ctrl(i);
177  else
178  {
179  cm_id_prev = m_jointCtrlModes_previous[i].id;
180  const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
181  assert(ctrl_prev.size()==m_numDofs);
182 
184 // SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
185 // " to "+toString(cm_id)+" alpha="+toString(alpha),MSG_TYPE_DEBUG);
186  s(i) = alpha*ctrl_prev(i) + (1-alpha)*ctrl(i);
188 
189  if(m_jointCtrlModesCountDown[i]==0)
190  {
191  SEND_MSG("Joint "+toString(i)+" changed ctrl mode from "+toString(cm_id_prev)+
192  " to "+toString(cm_id),MSG_TYPE_INFO);
194  }
195  }
196  }
197  }
198 
199  // usleep(1e6*m_sleep_time);
200  // if(m_sleep_time>=0.1)
201  // {
202  // for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
203  // {
204  // const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
205  // SEND_MSG(toString(iter)+") tau =" +toString(ctrl,1,4," ")+m_ctrlModes[i], MSG_TYPE_ERROR);
206  // }
207  // }
208 
209  return s;
210  }
211 
213  {
214  if(!m_initSucceeded)
215  {
216  SEND_WARNING_STREAM_MSG("Cannot compute signal u_safe before initialization!");
217  return s;
218  }
219  if(s.size()!=(Eigen::VectorXd::Index) m_numDofs)
220  s.resize(m_numDofs);
221 
222  const dynamicgraph::Vector& u = m_uSOUT(iter);
223  const dynamicgraph::Vector& ctrl_max = m_u_maxSIN(iter);
224 
225  for(size_t i=0; i<m_emergencyStopVector.size(); i++)
226  {
227  if ((* m_emergencyStopVector[i]).isPlugged() && (* m_emergencyStopVector[i])(iter))
228  {
230  SEND_MSG("t = " + toString(iter) + ": Emergency Stop has been triggered by an external entity: " + (*m_emergencyStopVector[i]).getName(), MSG_TYPE_ERROR);
231  }
232  }
233 
235  {
236  for(unsigned int i=0; i<m_numDofs; i++)
237  {
238 
239  if(fabs(u(i)) > ctrl_max(i))
240  {
242  SEND_MSG("t = " + toString(iter) + ": Joint " + toString(i) + " desired control is too large: "+ toString(u(i))+" > "+toString(ctrl_max(i)), MSG_TYPE_ERROR_STREAM);
243  break;
244  }
245  }
246  }
247 
248  s = u;
249 
251  s.setZero();
252 
253  return s;
254  }
255 
256  /* --- COMMANDS ---------------------------------------------------------- */
257 
258  void TalosControlManager::addCtrlMode(const string& name)
259  {
260  // check there is no other control mode with the same name
261  for(unsigned int i=0;i<m_ctrlModes.size(); i++)
262  if(name==m_ctrlModes[i])
263  return SEND_MSG("It already exists a control mode with name "+name, MSG_TYPE_ERROR);
264 
265  // create a new input signal to read the new control
266  m_ctrlInputsSIN.push_back(new SignalPtr<dynamicgraph::Vector, int>(NULL,
267  getClassName()+"("+getName()+")::input(dynamicgraph::Vector)::ctrl_"+name));
268 
269  // create a new output signal to specify which joints are controlled with the new
270  // control mode
271  m_jointsCtrlModesSOUT.push_back(new Signal<dynamicgraph::Vector, int>(
272  getClassName()+"("+getName()+")::output(dynamicgraph::Vector)::joints_ctrl_mode_"+name));
273 
274  // add the new control mode to the list of available control modes
275  m_ctrlModes.push_back(name);
276 
277  // register the new signals and add the new signal dependecy
278  Eigen::VectorXd::Index i = m_ctrlModes.size()-1;
279  m_uSOUT.addDependency(*m_ctrlInputsSIN[i]);
280  m_u_safeSOUT.addDependency(*m_ctrlInputsSIN[i]);
281  Entity::signalRegistration(*m_ctrlInputsSIN[i]);
282  Entity::signalRegistration(*m_jointsCtrlModesSOUT[i]);
284  }
285 
287  {
288  SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO);
289  }
290 
291 
292  void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
293  {
294  CtrlMode cm;
295  if(convertStringToCtrlMode(ctrlMode,cm)==false)
296  return;
297 
298  if(jointName=="all")
299  {
300  for(unsigned int i=0; i<m_numDofs; i++)
301  setCtrlMode(i,cm);
302  }
303  else
304  {
305  // decompose strings like "rk-rhp-lhp-..."
306  std::stringstream ss(jointName);
307  std::string item;
308  std::list<int> jIdList;
309  unsigned int i;
310  while (std::getline(ss, item, '-'))
311  {
312  SEND_MSG("parsed joint : "+item, MSG_TYPE_INFO);
313  if(convertJointNameToJointId(item,i))
314  jIdList.push_back(i);
315  }
316  for (std::list<int>::iterator it=jIdList.begin(); it != jIdList.end(); ++it)
317  setCtrlMode(*it,cm);
318  }
320  }
321 
322  void TalosControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
323  {
324  if(m_jointCtrlModesCountDown[jid]!=0)
325  return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
326  " because its previous ctrl mode transition has not terminated yet: "+
327  toString(m_jointCtrlModesCountDown[jid]), MSG_TYPE_ERROR);
328 
329  if(cm.id==m_jointCtrlModes_current[jid].id)
330  return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
331  " because it has already the specified ctrl mode", MSG_TYPE_ERROR);
332 
333  if(m_jointCtrlModes_current[jid].id<0)
334  {
335  // first setting of the control mode
336  m_jointCtrlModes_previous[jid] = cm;
337  m_jointCtrlModes_current[jid] = cm;
338  }
339  else
340  {
343  m_jointCtrlModes_current[jid] = cm;
344  }
345  }
346 
347  void TalosControlManager::getCtrlMode(const std::string& jointName)
348  {
349  if(jointName=="all")
350  {
351  stringstream ss;
352  for(unsigned int i=0; i<m_numDofs; i++)
353  ss<<toString(i) <<" "<<m_jointCtrlModes_current[i]<<"; ";
354  SEND_MSG(ss.str(),MSG_TYPE_INFO);
355  return;
356  }
357 
358  unsigned int i;
359  if(convertJointNameToJointId(jointName,i)==false)
360  return;
361  SEND_MSG("The control mode of joint "+jointName+" is "+m_jointCtrlModes_current[i].name,MSG_TYPE_INFO);
362  }
363 
365  {
366  getProfiler().reset_all();
368  }
369 
370 // void TalosControlManager::setStreamPrintPeriod(const double & s)
371 // {
372 // getLogger().setStreamPrintPeriod(s);
373 // }
374 
375  void TalosControlManager::setSleepTime(const double &seconds)
376  {
377  if(seconds<0.0)
378  return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
379  m_sleep_time = seconds;
380  }
381 
383  {
384  SEND_MSG("New emergency signal input emergencyStop_" + name + " created",MSG_TYPE_INFO);
385  // create a new input signal
386  m_emergencyStopVector.push_back(new SignalPtr<bool, int>(NULL, getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
387 
388  // register the new signals and add the new signal dependecy
389  Eigen::Index i = m_emergencyStopVector.size()-1;
390  m_u_safeSOUT.addDependency(* m_emergencyStopVector[i]);
391  Entity::signalRegistration(* m_emergencyStopVector[i]);
392  }
393 
394  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
395 
397  {
398  if (m_numDofs==0)
399  {
400  SEND_MSG("You should call init first. The size of the vector is unknown.", MSG_TYPE_ERROR);
401  return;
402  }
403 
405  for(unsigned int i=0; i<m_jointsCtrlModesSOUT.size(); i++)
406  {
407  for(unsigned int j=0; j<m_numDofs; j++)
408  {
409  cm(j) = 0;
410  if((unsigned int)m_jointCtrlModes_current[j].id == i)
411  cm(j) = 1;
412 
413  // during the transition between two ctrl modes they both result active
414  if(m_jointCtrlModesCountDown[j]>0 && (unsigned int)m_jointCtrlModes_previous[j].id == i)
415  cm(j) = 1;
416  }
417  m_jointsCtrlModesSOUT[i]->setConstant(cm);
418  }
419 
420  }
421 
422  bool TalosControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
423  {
424  // Check if the ctrl mode name exists
425  for(unsigned int i=0;i<m_ctrlModes.size(); i++)
426  if(name==m_ctrlModes[i])
427  {
428  cm = CtrlMode(i,name);
429  return true;
430  }
431  SEND_MSG("The specified control mode does not exist", MSG_TYPE_ERROR);
432  SEND_MSG("Possible control modes are: "+toString(m_ctrlModes), MSG_TYPE_INFO);
433  return false;
434  }
435 
436  bool TalosControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
437  {
438  // Check if the joint name exists
439  int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
440  jid += 6; // Take into account the FF
441  if (jid<0)
442  {
443  SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
444  std::stringstream ss;
445  for(size_t it=0; it<m_numDofs; it++)
446  ss<< toString(it) <<", ";
447  SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
448  return false;
449  }
450  id = (unsigned int )jid;
451  return true;
452  }
453 
454 /*
455  bool TalosControlManager::isJointInRange(unsigned int id, double q)
456  {
457  const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
458 
459  double jl= JL.lower;
460  if(q<jl)
461  {
462  SEND_MSG("Desired joint angle "+toString(q)+" is smaller than lower limit: "+toString(jl),MSG_TYPE_ERROR);
463  return false;
464  }
465  double ju = JL.upper;
466  if(q>ju)
467  {
468  SEND_MSG("Desired joint angle "+toString(q)+" is larger than upper limit: "+toString(ju),MSG_TYPE_ERROR);
469  return false;
470  }
471  return true;
472  }
473 */
474 
475 
476  /* ------------------------------------------------------------------- */
477  /* --- ENTITY -------------------------------------------------------- */
478  /* ------------------------------------------------------------------- */
479 
480 
481  void TalosControlManager::display(std::ostream& os) const
482  {
483  os << "TalosControlManager "<<getName();
484  try
485  {
486  getProfiler().report_all(3, os);
487  }
488  catch (ExceptionSignal e) {}
489  }
490 
491  } // namespace talos_balance
492  } // namespace sot
493 } // namespace dynamicgraph
std::vector< CtrlMode > m_jointCtrlModes_current
existing control modes
#define CTRL_MODE_TRANSITION_TIME_STEP
Number of time step to transition from one ctrl mode to another.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
std::vector< CtrlMode > m_jointCtrlModes_previous
control mode of the joints
STL namespace.
std::vector< int > m_jointCtrlModesCountDown
previous control mode of the joints
std::vector< dynamicgraph::SignalPtr< bool, int > *> m_emergencyStopVector
void setSleepTime(const double &seconds)
Commands related to joint name and joint id.
double m_dt
true if the entity has been successfully initialized
std::vector< dynamicgraph::Signal< dynamicgraph::Vector, int > * > m_jointsCtrlModesSOUT
emergency stop inputs. If one is true, control is set to zero forever
bool m_is_first_iter
true if an emergency condition as been triggered either by an other entity, or by control limit viola...
std::vector< std::string > m_ctrlModes
time to sleep at every iteration (to slow down simulation)
void reset_all()
Definition: statistics.cpp:72
void init(const double &dt, const std::string &robotRef)
#define OUTPUT_SIGNALS
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager, "TalosControlManager")
Statistics & getStatistics()
Definition: statistics.cpp:26
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
bool convertStringToCtrlMode(const std::string &name, CtrlMode &cm)
counters used for the transition between two ctrl modes
void addCtrlMode(const std::string &name)
safe motor control
std::vector< dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * > m_ctrlInputsSIN
#define INPUT_SIGNALS
void setCtrlMode(const std::string &jointName, const std::string &ctrlMode)