sot-talos-balance  1.5.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  // trigger computation of all ctrl inputs
158  for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
159  (*m_ctrlInputsSIN[i])(iter);
160 
161  int cm_id, cm_id_prev;
162  for(unsigned int i=0; i<m_numDofs; i++)
163  {
164  cm_id = m_jointCtrlModes_current[i].id;
165  if(cm_id<0)
166  {
167  SEND_MSG("You forgot to set the control mode of joint "+toString(i), MSG_TYPE_ERROR_STREAM);
168  continue;
169  }
170 
171  const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
172  assert(ctrl.size()==m_numDofs);
173 
174  if(m_jointCtrlModesCountDown[i]==0)
175  s(i) = ctrl(i);
176  else
177  {
178  cm_id_prev = m_jointCtrlModes_previous[i].id;
179  const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
180  assert(ctrl_prev.size()==m_numDofs);
181 
183 // SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
184 // " to "+toString(cm_id)+" alpha="+toString(alpha),MSG_TYPE_DEBUG);
185  s(i) = alpha*ctrl_prev(i) + (1-alpha)*ctrl(i);
187 
188  if(m_jointCtrlModesCountDown[i]==0)
189  {
190  SEND_MSG("Joint "+toString(i)+" changed ctrl mode from "+toString(cm_id_prev)+
191  " to "+toString(cm_id),MSG_TYPE_INFO);
193  }
194  }
195  }
196  }
197 
198  // usleep(1e6*m_sleep_time);
199  // if(m_sleep_time>=0.1)
200  // {
201  // for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
202  // {
203  // const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
204  // SEND_MSG(toString(iter)+") tau =" +toString(ctrl,1,4," ")+m_ctrlModes[i], MSG_TYPE_ERROR);
205  // }
206  // }
207 
208  return s;
209  }
210 
212  {
213  if(!m_initSucceeded)
214  {
215  SEND_WARNING_STREAM_MSG("Cannot compute signal u_safe before initialization!");
216  return s;
217  }
218 
219  const dynamicgraph::Vector& u = m_uSOUT(iter);
220  const dynamicgraph::Vector& ctrl_max = m_u_maxSIN(iter);
221 
222  for(size_t i=0; i<m_emergencyStopVector.size(); i++)
223  {
224  if ((* m_emergencyStopVector[i]).isPlugged() && (* m_emergencyStopVector[i])(iter))
225  {
227  SEND_MSG("Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
228  }
229  }
230 
232  {
233  for(unsigned int i=0; i<m_numDofs; i++)
234  {
235 
236  if(fabs(u(i)) > ctrl_max(i))
237  {
239  SEND_MSG("Joint " + toString(i) + " desired control is too large: "+ toString(u(i))+" > "+toString(ctrl_max(i)), MSG_TYPE_ERROR_STREAM);
240  break;
241  }
242  }
243  }
244 
245  s = u;
246 
248  s.setZero();
249 
250  return s;
251  }
252 
253  /* --- COMMANDS ---------------------------------------------------------- */
254 
255  void TalosControlManager::addCtrlMode(const string& name)
256  {
257  // check there is no other control mode with the same name
258  for(unsigned int i=0;i<m_ctrlModes.size(); i++)
259  if(name==m_ctrlModes[i])
260  return SEND_MSG("It already exists a control mode with name "+name, MSG_TYPE_ERROR);
261 
262  // create a new input signal to read the new control
263  m_ctrlInputsSIN.push_back(new SignalPtr<dynamicgraph::Vector, int>(NULL,
264  getClassName()+"("+getName()+")::input(dynamicgraph::Vector)::ctrl_"+name));
265 
266  // create a new output signal to specify which joints are controlled with the new
267  // control mode
268  m_jointsCtrlModesSOUT.push_back(new Signal<dynamicgraph::Vector, int>(
269  getClassName()+"("+getName()+")::output(dynamicgraph::Vector)::joints_ctrl_mode_"+name));
270 
271  // add the new control mode to the list of available control modes
272  m_ctrlModes.push_back(name);
273 
274  // register the new signals and add the new signal dependecy
275  Eigen::VectorXd::Index i = m_ctrlModes.size()-1;
276  m_uSOUT.addDependency(*m_ctrlInputsSIN[i]);
277  m_u_safeSOUT.addDependency(*m_ctrlInputsSIN[i]);
278  Entity::signalRegistration(*m_ctrlInputsSIN[i]);
279  Entity::signalRegistration(*m_jointsCtrlModesSOUT[i]);
281  }
282 
284  {
285  SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO);
286  }
287 
288 
289  void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
290  {
291  CtrlMode cm;
292  if(convertStringToCtrlMode(ctrlMode,cm)==false)
293  return;
294 
295  if(jointName=="all")
296  {
297  for(unsigned int i=0; i<m_numDofs; i++)
298  setCtrlMode(i,cm);
299  }
300  else
301  {
302  // decompose strings like "rk-rhp-lhp-..."
303  std::stringstream ss(jointName);
304  std::string item;
305  std::list<int> jIdList;
306  unsigned int i;
307  while (std::getline(ss, item, '-'))
308  {
309  SEND_MSG("parsed joint : "+item, MSG_TYPE_INFO);
310  if(convertJointNameToJointId(item,i))
311  jIdList.push_back(i);
312  }
313  for (std::list<int>::iterator it=jIdList.begin(); it != jIdList.end(); ++it)
314  setCtrlMode(*it,cm);
315  }
317  }
318 
319  void TalosControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
320  {
321  if(m_jointCtrlModesCountDown[jid]!=0)
322  return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
323  " because its previous ctrl mode transition has not terminated yet: "+
324  toString(m_jointCtrlModesCountDown[jid]), MSG_TYPE_ERROR);
325 
326  if(cm.id==m_jointCtrlModes_current[jid].id)
327  return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
328  " because it has already the specified ctrl mode", MSG_TYPE_ERROR);
329 
330  if(m_jointCtrlModes_current[jid].id<0)
331  {
332  // first setting of the control mode
333  m_jointCtrlModes_previous[jid] = cm;
334  m_jointCtrlModes_current[jid] = cm;
335  }
336  else
337  {
340  m_jointCtrlModes_current[jid] = cm;
341  }
342  }
343 
344  void TalosControlManager::getCtrlMode(const std::string& jointName)
345  {
346  if(jointName=="all")
347  {
348  stringstream ss;
349  for(unsigned int i=0; i<m_numDofs; i++)
350  ss<<toString(i) <<" "<<m_jointCtrlModes_current[i]<<"; ";
351  SEND_MSG(ss.str(),MSG_TYPE_INFO);
352  return;
353  }
354 
355  unsigned int i;
356  if(convertJointNameToJointId(jointName,i)==false)
357  return;
358  SEND_MSG("The control mode of joint "+jointName+" is "+m_jointCtrlModes_current[i].name,MSG_TYPE_INFO);
359  }
360 
362  {
363  getProfiler().reset_all();
365  }
366 
367 // void TalosControlManager::setStreamPrintPeriod(const double & s)
368 // {
369 // getLogger().setStreamPrintPeriod(s);
370 // }
371 
372  void TalosControlManager::setSleepTime(const double &seconds)
373  {
374  if(seconds<0.0)
375  return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
376  m_sleep_time = seconds;
377  }
378 
380  {
381  SEND_MSG("New emergency signal input emergencyStop_" + name + " created",MSG_TYPE_INFO);
382  // create a new input signal
383  m_emergencyStopVector.push_back(new SignalPtr<bool, int>(NULL, getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
384 
385  // register the new signals and add the new signal dependecy
386  Eigen::Index i = m_emergencyStopVector.size()-1;
387  m_u_safeSOUT.addDependency(* m_emergencyStopVector[i]);
388  Entity::signalRegistration(* m_emergencyStopVector[i]);
389  }
390 
391  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
392 
394  {
395  if (m_numDofs==0)
396  {
397  SEND_MSG("You should call init first. The size of the vector is unknown.", MSG_TYPE_ERROR);
398  return;
399  }
400 
402  for(unsigned int i=0; i<m_jointsCtrlModesSOUT.size(); i++)
403  {
404  for(unsigned int j=0; j<m_numDofs; j++)
405  {
406  cm(j) = 0;
407  if((unsigned int)m_jointCtrlModes_current[j].id == i)
408  cm(j) = 1;
409 
410  // during the transition between two ctrl modes they both result active
411  if(m_jointCtrlModesCountDown[j]>0 && (unsigned int)m_jointCtrlModes_previous[j].id == i)
412  cm(j) = 1;
413  }
414  m_jointsCtrlModesSOUT[i]->setConstant(cm);
415  }
416 
417  }
418 
419  bool TalosControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
420  {
421  // Check if the ctrl mode name exists
422  for(unsigned int i=0;i<m_ctrlModes.size(); i++)
423  if(name==m_ctrlModes[i])
424  {
425  cm = CtrlMode(i,name);
426  return true;
427  }
428  SEND_MSG("The specified control mode does not exist", MSG_TYPE_ERROR);
429  SEND_MSG("Possible control modes are: "+toString(m_ctrlModes), MSG_TYPE_INFO);
430  return false;
431  }
432 
433  bool TalosControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
434  {
435  // Check if the joint name exists
436  int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
437  jid += 6; // Take into account the FF
438  if (jid<0)
439  {
440  SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
441  std::stringstream ss;
442  for(size_t it=0; it<m_numDofs; it++)
443  ss<< toString(it) <<", ";
444  SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
445  return false;
446  }
447  id = (unsigned int )jid;
448  return true;
449  }
450 
451 /*
452  bool TalosControlManager::isJointInRange(unsigned int id, double q)
453  {
454  const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
455 
456  double jl= JL.lower;
457  if(q<jl)
458  {
459  SEND_MSG("Desired joint angle "+toString(q)+" is smaller than lower limit: "+toString(jl),MSG_TYPE_ERROR);
460  return false;
461  }
462  double ju = JL.upper;
463  if(q>ju)
464  {
465  SEND_MSG("Desired joint angle "+toString(q)+" is larger than upper limit: "+toString(ju),MSG_TYPE_ERROR);
466  return false;
467  }
468  return true;
469  }
470 */
471 
472 
473  /* ------------------------------------------------------------------- */
474  /* --- ENTITY -------------------------------------------------------- */
475  /* ------------------------------------------------------------------- */
476 
477 
478  void TalosControlManager::display(std::ostream& os) const
479  {
480  os << "TalosControlManager "<<getName();
481  try
482  {
483  getProfiler().report_all(3, os);
484  }
485  catch (ExceptionSignal e) {}
486  }
487 
488  } // namespace talos_balance
489  } // namespace sot
490 } // 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)