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