sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
trace-player.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #include <tsid/utils/stop-watch.hpp>
7 #include <tsid/utils/statistics.hpp>
8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
12 
13 namespace dynamicgraph {
14 namespace sot {
15 namespace torque_control {
16 namespace dynamicgraph = ::dynamicgraph;
17 using namespace dynamicgraph;
18 using namespace dynamicgraph::command;
19 using namespace std;
20 using namespace dynamicgraph::sot::torque_control;
21 
25 
26 /* --- DG FACTORY ---------------------------------------------------- */
28 
29 /* ------------------------------------------------------------------- */
30 /* --- CONSTRUCTION -------------------------------------------------- */
31 /* ------------------------------------------------------------------- */
32 TracePlayer::TracePlayer(const std::string& name) : Entity(name), CONSTRUCT_SIGNAL_OUT(trigger, int, sotNOSIGNAL) {
33  Entity::signalRegistration(m_triggerSOUT);
34 
35  /* Commands. */
36  addCommand("addOutputSignal",
37  makeCommandVoid2(
39  docCommandVoid2("Add a new output signal", "Name of the text file where to read the data (string)",
40  "Name of the output signal (string)")));
41 
42  addCommand("playNext",
43  makeCommandVoid0(*this, &TracePlayer::playNext, docCommandVoid0("Update all the output signals.")));
44 
45  addCommand("rewind", makeCommandVoid0(*this, &TracePlayer::rewind, docCommandVoid0("Rewind all the data.")));
46 
47  addCommand("clear", makeCommandVoid0(*this, &TracePlayer::clear, docCommandVoid0("Clear all the output signals.")));
48 }
49 
50 /* ------------------------------------------------------------------- */
51 /* --- SIGNALS ------------------------------------------------------- */
52 /* ------------------------------------------------------------------- */
53 
55  std::string astr = toString(iter);
56  playNext();
57  return s;
58 }
59 
60 /* --- COMMANDS ---------------------------------------------------------- */
61 
62 void TracePlayer::addOutputSignal(const string& fileName, const string& signalName) {
63  // check there is no other signal with the same name
64  if (m_outputSignals.find(signalName) != m_outputSignals.end())
65  return SEND_MSG("It already exists a signal with name " + signalName, MSG_TYPE_ERROR);
66 
67  // read the text file
68  std::ifstream datafile(fileName.c_str());
69  if (datafile.fail()) return SEND_MSG("Error trying to read the file " + fileName, MSG_TYPE_ERROR);
70 
71  const unsigned int SIZE = 1024;
72  char buffer[SIZE];
73  std::vector<double> newline;
74  int nbLines = 0;
75  bool firstIter = true;
76  std::size_t size = -1;
77  string fileNameShort = fileName.substr(1 + fileName.find_last_of("/"));
78  while (datafile.good()) {
79  datafile.getline(buffer, SIZE);
80  const std::size_t gcount = datafile.gcount();
81  if (gcount >= SIZE)
82  return SEND_MSG("Read error: line " + toString(nbLines) + " too long in file " + fileNameShort, MSG_TYPE_ERROR);
83 
84  std::istringstream iss(buffer);
85  newline.clear();
86  double x;
87  iss >> x; // discard the first value, which is the time step
88  while (1) {
89  iss >> x;
90  if (iss.fail()) break;
91  newline.push_back(x);
92  }
93 
94  if (newline.size() > 0) {
95  if (firstIter)
96  size = newline.size();
97  else if (size != newline.size()) {
98  SEND_MSG("In file " + fileNameShort + " nb of elements in each line changed from " + toString(size) + " to " +
99  toString(newline.size()) + " at line " + toString(nbLines),
100  MSG_TYPE_WARNING);
101  size = newline.size();
102  }
103  m_data[signalName].push_back(Eigen::Map<Vector>(&newline[0], newline.size()));
104  nbLines++;
105  }
106  }
107  SEND_MSG(
108  "Finished reading " + toString(nbLines) + " lines of " + toString(size) + " elements from file " + fileNameShort,
109  MSG_TYPE_INFO);
110  m_dataPointers[signalName] = m_data[signalName].begin();
111 
112  // create a new output signal
113  m_outputSignals[signalName] =
114  new OutputSignalType(getClassName() + "(" + getName() + ")::output(dynamicgraph::Vector)::" + signalName);
115 
116  // register the new signal
117  m_triggerSOUT.addDependency(*m_outputSignals[signalName]);
118  Entity::signalRegistration(*m_outputSignals[signalName]);
119 }
120 
122  typedef std::map<std::string, OutputSignalType*>::iterator it_type;
123  for (it_type it = m_outputSignals.begin(); it != m_outputSignals.end(); it++) {
124  const string& signalName = it->first;
125  OutputSignalType* signal = it->second;
126  DataPointerType& dataPointer = m_dataPointers[signalName];
127  const DataHistoryType& dataSet = m_data[signalName];
128 
129  if (dataPointer != dataSet.end()) ++dataPointer;
130 
131  if (dataPointer == dataSet.end())
132  SEND_WARNING_STREAM_MSG("Reached end of dataset for signal " + signalName);
133  else
134  signal->setConstant(*dataPointer);
135  }
136 }
137 
139  typedef std::map<std::string, DataPointerType>::iterator it_type;
140  for (it_type it = m_dataPointers.begin(); it != m_dataPointers.end(); it++) {
141  const string& signalName = it->first;
142  DataPointerType& dataPointer = it->second;
143  const DataHistoryType& dataSet = m_data[signalName];
144  dataPointer = dataSet.begin();
145  }
146 }
147 
149  m_data.clear();
150  m_dataPointers.clear();
151  m_outputSignals.clear();
152 }
153 
154 /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
155 
156 /* ------------------------------------------------------------------- */
157 /* --- ENTITY -------------------------------------------------------- */
158 /* ------------------------------------------------------------------- */
159 
160 void TracePlayer::display(std::ostream& os) const {
161  os << "TracePlayer " << getName();
162  try {
163  getProfiler().report_all(3, os);
164  } catch (ExceptionSignal e) {
165  }
166 }
167 } // namespace torque_control
168 } // namespace sot
169 } // namespace dynamicgraph
Entity to play data saved using a Tracer.
Definition: trace-player.hh:58
std::map< std::string, DataPointerType > m_dataPointers
Definition: trace-player.hh:92
virtual void display(std::ostream &os) const
dynamicgraph::Signal< dynamicgraph::Vector, int > OutputSignalType
Definition: trace-player.hh:69
void addOutputSignal(const std::string &fileName, const std::string &signalName)
std::map< std::string, DataHistoryType > m_data
Definition: trace-player.hh:91
DataHistoryType::const_iterator DataPointerType
Definition: trace-player.hh:89
std::map< std::string, OutputSignalType * > m_outputSignals
Definition: trace-player.hh:70
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22