CommonTools.cpp 10.1 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
student's avatar
student committed
2
 * Copyright 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
 *
 * Andrei  Herdt
 * Olivier Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
student's avatar
student committed
22
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
23
24
25
26
27
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */

#ifdef UNIX
#include <stdlib.h>
28
#include <sys/time.h>
Thomas Moulard's avatar
Thomas Moulard committed
29
30
31
#endif /*UNIX*/

#ifdef WIN32
32
#include "portability/gettimeofday.hh"
33
#include <Windows.h>
Thomas Moulard's avatar
Thomas Moulard committed
34
35
36
#endif /*WIN32*/

#include <fstream>
37
#include <sstream>
Thomas Moulard's avatar
Thomas Moulard committed
38

39
#include "CommonTools.hh"
40
#include "Debug.hh"
Thomas Moulard's avatar
Thomas Moulard committed
41

42
#include <jrl/walkgen/patterngeneratorinterface.hh>
Thomas Moulard's avatar
Thomas Moulard committed
43
44
45
46
47
48

#include "TestFootPrintPGInterfaceData.h"

using namespace std;

namespace PatternGeneratorJRL {
49
50
51
namespace TestSuite {

double filterprecision(double adb) {
52
53
  if (fabs(adb) < 1e-7)
    return 0.0;
54
55
56
57
58
59
60
61

  double ladb2 = adb * 1e7;
  double lintadb2 = trunc(ladb2);
  return lintadb2 / 1e7;
}

void CommonInitialization(PatternGeneratorInterface &aPGI) {
  const unsigned int nbMethod = 13;
62
63
64
65
66
67
68
69
70
71
72
73
74
75
  const char lBuffer[nbMethod][256] = {
      ":comheight 0.876681",
      ":samplingperiod 0.005",
      ":previewcontroltime 1.6",
      ":omega 0.0",
      ":stepheight 0.07",
      ":singlesupporttime 0.78",
      ":doublesupporttime 0.02",
      ":armparameters 0.5",
      ":LimitsFeasibility 0.0",
      ":ZMPShiftParameters 0.015 0.015 0.015 0.015",
      ":TimeDistributionParameters 2.0 3.7 1.7 3.0",
      ":UpperBodyMotionParameters -0.1 -1.0 0.0",
      ":useDynamicFilter false"};
76
77
78
79
80
81
82
83
84
85
86
87

  for (unsigned int i = 0; i < nbMethod; i++) {
    std::istringstream strm(lBuffer[i]);
    aPGI.ParseCmd(strm);
  }
  // Evaluate current state of the robot in the PG.
  COMState lStartingCOMPosition;
  Eigen::Vector3d lStartingZMPPosition;
  Eigen::Matrix<double, 6, 1> lStartingWaistPose;
  FootAbsolutePosition InitLeftFootAbsPos;
  FootAbsolutePosition InitRightFootAbsPos;

88
89
  aPGI.EvaluateStartingState(lStartingCOMPosition, lStartingZMPPosition,
                             lStartingWaistPose, InitLeftFootAbsPos,
90
91
                             InitRightFootAbsPos);

92
93
  ODEBUG("Starting COM Position: " << lStartingCOMPosition.x[0] << " "
                                   << lStartingCOMPosition.y[0] << " "
94
95
                                   << lStartingCOMPosition.z[0]);

96
97
  ODEBUG("Starting Waist Position: " << lStartingWaistPose[0] << " "
                                     << lStartingWaistPose[1] << " "
98
99
                                     << lStartingWaistPose[2]);

100
101
  ODEBUG("Starting ZMP Position: " << lStartingZMPPosition[0] << " "
                                   << lStartingZMPPosition[1] << " "
102
103
                                   << lStartingZMPPosition[2]);

104
105
106
107
  ODEBUG("Starting Left Foot Pos: "
         << InitLeftFootAbsPos.x << " " << InitLeftFootAbsPos.y << " "
         << InitLeftFootAbsPos.z << " " << InitLeftFootAbsPos.theta << " "
         << InitLeftFootAbsPos.omega << " " << InitLeftFootAbsPos.omega2);
108

109
110
111
112
  ODEBUG("Starting Right Foot Pos: "
         << InitRightFootAbsPos.x << " " << InitRightFootAbsPos.y << " "
         << InitRightFootAbsPos.z << " " << InitRightFootAbsPos.theta << " "
         << InitRightFootAbsPos.omega << " " << InitRightFootAbsPos.omega2);
113
114
}

115
116
117
void getOptions(int argc, char *argv[], string &urdfFullPath,
                string &srdfFullPath,
                unsigned int &) // TestProfil)
118
{
119
120
121
122
123
124
125
126
  std::cout << "Nb of entries: " << argc << std::endl;
  if (argc != 3) {
    urdfFullPath = URDF_FULL_PATH;
    srdfFullPath = SRDF_FULL_PATH;
  } else {
    urdfFullPath = argv[1];
    srdfFullPath = argv[2];
  }
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
}

OneStep::OneStep() {
  m_PR = 0;
  m_ZMPTarget.resize(3);
  m_NbOfIt = 0;
  memset(&m_LeftFootPosition, 0, sizeof(m_LeftFootPosition));
  memset(&m_RightFootPosition, 0, sizeof(m_RightFootPosition));
  memset(&m_finalCOMPosition, 0, sizeof(m_finalCOMPosition));

  fillInDebugVectorDoc();
}

void OneStep::fillInDebugVectorDoc() {
  /// Building vector of documentation strings.
  const char *docInit[15] = {"Time",
                             "CoM - X",
                             "CoM - Y",
                             "CoM - Z",
                             "Yaw",
                             "dCoM - X",
                             "dCoM - Y",
                             "dCoM - Z",
                             "dCoM - Yaw",
                             "ddCoM - X",
                             "ddCoM - Y",
                             "ddCoM - Z",
                             "ddCoM - Yaw",
                             "ZMP X in Waist Ref",
                             "ZMP Y in Waist Ref"};

158
159
  for (std::size_t i = 0; i < 15; i++)
    m_DebugStrings.push_back(docInit[i]);
160
161

  const char *footNames[2] = {"Left Foot", "Right Foot"};
162
163
164
165
  const char *docFootInit[14] = {
      " - Pos X",  " - Pos Y",   " - Pos Z",   " - Vel dX",  " - Vel dY",
      " - Vel dZ", " - Acc ddX", " - Acc ddY", " - Acc ddZ", " - Yaw",
      " - dYaw",   " - ddYaw",   " - Roll",    " - Pitch"};
166
167
168
169
170
171
  for (std::size_t footId = 0; footId < 2; footId++) {
    for (std::size_t docFootId = 0; docFootId < 14; docFootId++) {
      std::string aDoc = footNames[footId];
      aDoc = aDoc + " ";
      aDoc = aDoc + std::string(docFootInit[docFootId]);
      m_DebugStrings.push_back(aDoc);
172
    }
173
  }
olivier-stasse's avatar
olivier-stasse committed
174

175
176
  const char *docInit2[2] = {"ZMP Target X - world ref",
                             " ZMP Target Y - world ref"};
olivier-stasse's avatar
olivier-stasse committed
177

178
179
  for (std::size_t i = 0; i < 2; i++)
    m_DebugStrings.push_back(docInit2[i]);
180

181
182
  if (m_PR != 0) {
    Eigen::VectorXd &currentConfiguration = m_PR->currentRPYConfiguration();
183

184
185
186
187
    for (unsigned int i = 0; i < currentConfiguration.size(); i++) {
      std::ostringstream aDoc(" conf. ");
      aDoc << i;
      m_DebugStrings.push_back(aDoc.str());
Thomas Moulard's avatar
Thomas Moulard committed
188
    }
189
190
191
  }
}

192
193
void OneStep::fillInDebugVectorFoot(FootAbsolutePosition &aFootAbsolutePosition,
                                    std::size_t &lindex) {
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
  m_DebugVector[lindex++] = aFootAbsolutePosition.x;
  m_DebugVector[lindex++] = aFootAbsolutePosition.y;
  m_DebugVector[lindex++] = aFootAbsolutePosition.z;
  m_DebugVector[lindex++] = aFootAbsolutePosition.dx;
  m_DebugVector[lindex++] = aFootAbsolutePosition.dy;
  m_DebugVector[lindex++] = aFootAbsolutePosition.dz;
  m_DebugVector[lindex++] = aFootAbsolutePosition.ddx;
  m_DebugVector[lindex++] = aFootAbsolutePosition.ddy;
  m_DebugVector[lindex++] = aFootAbsolutePosition.ddz;
  m_DebugVector[lindex++] = aFootAbsolutePosition.theta;
  m_DebugVector[lindex++] = aFootAbsolutePosition.dtheta;
  m_DebugVector[lindex++] = aFootAbsolutePosition.ddtheta;
  m_DebugVector[lindex++] = aFootAbsolutePosition.omega;
  m_DebugVector[lindex++] = aFootAbsolutePosition.omega2;
}

void OneStep::fillInDebugVector() {
  std::size_t lindex = 0;
  Eigen::VectorXd &currentConfiguration = m_PR->currentRPYConfiguration();

  Eigen::Index nq = currentConfiguration.size();
215
216
  if (m_DebugVector.size() == 0)
    m_DebugVector.resize(17 + 14 * 2 + nq);
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245

  /// Time
  m_DebugVector[lindex++] = ((double)m_NbOfIt) * 0.005;
  /// CoM Position
  m_DebugVector[lindex++] = m_finalCOMPosition.x[0];
  m_DebugVector[lindex++] = m_finalCOMPosition.y[0];
  m_DebugVector[lindex++] = m_finalCOMPosition.z[0];
  m_DebugVector[lindex++] = m_finalCOMPosition.yaw[0];
  m_DebugVector[lindex++] = m_finalCOMPosition.x[1];
  m_DebugVector[lindex++] = m_finalCOMPosition.y[1];
  m_DebugVector[lindex++] = m_finalCOMPosition.z[1];
  m_DebugVector[lindex++] = m_finalCOMPosition.yaw[1];
  m_DebugVector[lindex++] = m_finalCOMPosition.x[2];
  m_DebugVector[lindex++] = m_finalCOMPosition.y[2];
  m_DebugVector[lindex++] = m_finalCOMPosition.z[2];
  m_DebugVector[lindex++] = m_finalCOMPosition.yaw[2];

  /// ZMP Target
  m_DebugVector[lindex++] = m_ZMPTarget(0);
  m_DebugVector[lindex++] = m_ZMPTarget(1);
  /// Left Foot Position
  fillInDebugVectorFoot(m_LeftFootPosition, lindex);
  /// Right Foot position
  fillInDebugVectorFoot(m_RightFootPosition, lindex);

  assert(m_PR != NULL);

  /// ZMP Target in absolute reference
  m_DebugVector[lindex++] = m_ZMPTarget(0) * cos(currentConfiguration(5)) -
246
247
                            m_ZMPTarget(1) * sin(currentConfiguration(5)) +
                            currentConfiguration(0);
248
  m_DebugVector[lindex++] = m_ZMPTarget(0) * sin(currentConfiguration(5)) +
249
250
                            m_ZMPTarget(1) * cos(currentConfiguration(5)) +
                            currentConfiguration(1);
251
252

  /// Saving configuration
253
254
  for (unsigned int i = 0; i < currentConfiguration.size(); i++)
    m_DebugVector[lindex++] = currentConfiguration(i);
255
256
257
}

void OneStep::fillInDebugFileContent(std::ofstream &aof) {
258
259
  for (std::size_t i = 0; i < m_DebugVector.size(); i++)
    aof << filterprecision(m_DebugVector[i]) << " ";
260
261
262
263
264
265
266
}

void OneStep::fillInDebugFile() {
  /// Store data
  fillInDebugVector();

  ofstream aof;
267
268
  
  /// Create filename
269
270
271
272
  string aFileName;
  assert(!m_TestName.empty());
  aFileName = m_TestName;
  aFileName += "TestFGPI.dat";
273
274
275
276
277
278
279
280
281
282
283

  if (m_NbOfIt == 0) {
    /// Write description file if this is the first iteration
    writeDescriptionFile();
    
    /// Erase the file if we start.
    aof.open(aFileName.c_str(), ofstream::out);
    aof.close();
  }

  /// Open file additively
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
  aof.open(aFileName.c_str(), ofstream::app);
  aof.precision(8);
  aof.setf(ios::scientific, ios::floatfield);
  /// Write down the file
  fillInDebugFileContent(aof);
  aof << endl;
  aof.close();
}

void OneStep::writeDescriptionFile() {
  ofstream aof;
  string aFileName;
  assert(!m_TestName.empty());
  aFileName = m_TestName;
  aFileName += "TestFGPI_description.dat";
  aof.open(aFileName.c_str(), ofstream::out);

301
302
  for (std::size_t i = 0; i < m_DebugStrings.size(); i++)
    aof << i << " - " << m_DebugStrings[i] << std::endl;
303
304
  aof.close();
}
305
306
} // namespace TestSuite
} // namespace PatternGeneratorJRL