GenerateMotionFromKineoWorks.cpp 9.8 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2005, 2006, 2007, 2008, 2009, 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
 *
 * 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/>.
 *
21
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
22
23
24
25
26
27
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/* \file GenerationMotionFromKineoWorks
   This class can inherited for creating the trajectory of the robot's joints
   given a path provided by KineoWorks. This path can be based on a partial
   model of the robot. The link between this partial model is given by
28
   an auxiliary file, and the redefinition of the virtual
Thomas Moulard's avatar
Thomas Moulard committed
29
30
31
   functions of this class.
*/
#include <fstream>
32
#include <iostream>
Thomas Moulard's avatar
Thomas Moulard committed
33

34
#include <MotionGeneration/GenerateMotionFromKineoWorks.hh>
35
36
#include <PreviewControl/PreviewControl.hh>
#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
Thomas Moulard's avatar
Thomas Moulard committed
37

38
39
#define _DEBUG_
namespace PatternGeneratorJRL {
Thomas Moulard's avatar
Thomas Moulard committed
40

41
42
43
GenerateMotionFromKineoWorks::GenerateMotionFromKineoWorks() {
  m_NbOfDOFsFromKW = 0;
}
Thomas Moulard's avatar
Thomas Moulard committed
44

45
GenerateMotionFromKineoWorks::~GenerateMotionFromKineoWorks() {}
Thomas Moulard's avatar
Thomas Moulard committed
46

47
48
int GenerateMotionFromKineoWorks::ReadPartialModel(string aFileName) {
  std::ifstream aif;
Thomas Moulard's avatar
Thomas Moulard committed
49

50
51
52
53
54
55
56
  aif.open(aFileName.c_str(), std::ifstream::in);
  if (aif.is_open()) {
    aif >> m_NbOfDOFsFromKW;
    if (m_NbOfDOFsFromKW < 0) {
      m_NbOfDOFsFromKW = 0;
      return -1;
    }
Thomas Moulard's avatar
Thomas Moulard committed
57

58
59
60
61
62
    m_IndexFromKWToRobot.resize(m_NbOfDOFsFromKW);
    for (int i = 0; i < m_NbOfDOFsFromKW; i++) {
      aif >> m_IndexFromKWToRobot[i];
    }
    aif.close();
Thomas Moulard's avatar
Thomas Moulard committed
63
  }
64
65
  return 0;
}
Thomas Moulard's avatar
Thomas Moulard committed
66

67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
int GenerateMotionFromKineoWorks::ReadKineoWorksPath(string aFileName) {
  std::ifstream aif;

  aif.open(aFileName.c_str(), std::ifstream::in);
  if (aif.is_open()) {
    string atmp;
    aif >> atmp;
    if (atmp != "KWVersion") {
      cerr << "Not a Kineoworks' path file. Aborted." << endl;
      return -1;
    }

    double version;
    aif >> version;
    if (version != 2.0) {
      cerr << "Not version 2.0. Do not understand the information. "
           << "Aborted. " << endl;
      return -1;
    }
    aif >> atmp;
    if (atmp != "{") {
      cerr << "{ expected. Aborted. " << endl;
      return -1;
    }

    aif >> atmp;
    if (atmp != "SteeringMethodSelect") {
      cerr << "SteeringMethodSelect expected. Aborted. " << endl;
      return -1;
    }

    aif >> m_SteeringMethod;

    aif >> atmp;

    if (atmp != "Nodes") {
      cerr << "Nodes expected. Aborted. " << endl;
      return -1;
    }

    aif >> atmp;

    if (atmp != "[") {
      cerr << "] expected. Aborted. " << endl;
      return -1;
    }

    do {
      aif >> atmp;

      if (atmp[0] == '(') {
        string FirstJt = atmp.substr(1, atmp.length() - 1);
        KWNode aNode;
        aNode.Joints.resize(m_NbOfDOFsFromKW);
        aNode.Joints[0] = atof(FirstJt.c_str());
        for (int j = 1; j < m_NbOfDOFsFromKW; j++) {
          aif >> aNode.Joints[j];
        }
        aif >> atmp; // read ")"
        if (atmp != ")") {
          cerr << " ) expected. Aborted. " << atmp << endl;
          return -1;
        }
        m_Path.insert(m_Path.end(), aNode);
Thomas Moulard's avatar
Thomas Moulard committed
131
      }
132
    } while (atmp != "]");
Thomas Moulard's avatar
Thomas Moulard committed
133

134
    aif >> atmp;
Thomas Moulard's avatar
Thomas Moulard committed
135

136
137
138
139
140
141
    if (atmp != "}") {
      cerr << "} expected. Aborted. " << endl;
      return -1;
    }

    aif.close();
Thomas Moulard's avatar
Thomas Moulard committed
142
  }
143
144
  return 0;
}
Thomas Moulard's avatar
Thomas Moulard committed
145

146
147
148
149
150
151
152
void GenerateMotionFromKineoWorks::DisplayModelAndPath() {
  cout << "Partial model " << endl;
  cout << "Nb of DOFs: " << m_NbOfDOFsFromKW << endl;
  cout << "Correspondance between local DOFs and robot's ones." << endl;
  for (unsigned int i = 0; i < m_IndexFromKWToRobot.size(); i++) {
    cout << i << " : " << m_IndexFromKWToRobot[i] << endl;
  }
Thomas Moulard's avatar
Thomas Moulard committed
153

154
155
156
157
158
159
160
161
  cout << "KW Path " << endl;
  cout << "Steering Path " << m_SteeringMethod << endl;
  for (unsigned int i = 0; i < m_Path.size(); i++) {
    for (unsigned int j = 0; j < m_Path[i].Joints.size(); j++)
      cout << m_Path[i].Joints[j] << " ";
    cout << endl;
  }
}
162

163
164
165
166
167
168
169
void GenerateMotionFromKineoWorks::CreateBufferFirstPreview(
    deque<ZMPPosition> &ZMPRefBuffer) {
  deque<ZMPPosition> aFIFOZMPRefPositions;
  Eigen::MatrixXd aPC1x;
  Eigen::MatrixXd aPC1y;
  double aSxzmp, aSyzmp;
  double aZmpx2, aZmpy2;
170

171
172
173
  // Initialize local and object scope buffers.
  for (unsigned int i = 0; i < m_NL; i++)
    aFIFOZMPRefPositions.push_back(ZMPRefBuffer[i]);
174

175
  m_COMBuffer.resize(ZMPRefBuffer.size() - m_NL);
Thomas Moulard's avatar
Thomas Moulard committed
176

177
178
179
  // use accumulated zmp error  of preview control so far
  aSxzmp = 0.0; // m_sxzmp;
  aSyzmp = 0.0; // m_syzmp;
Thomas Moulard's avatar
Thomas Moulard committed
180

181
182
  aPC1x.resize(3, 1);
  aPC1y.resize(3, 1);
Thomas Moulard's avatar
Thomas Moulard committed
183

184
185
186
187
188
189
190
191
  aPC1x(0, 0) = 0;
  aPC1x(1, 0) = 0;
  aPC1x(2, 0) = 0;
  aPC1y(0, 0) = 0;
  aPC1y(1, 0) = 0;
  aPC1y(2, 0) = 0;

  // create the extra COMbuffer
192

Thomas Moulard's avatar
Thomas Moulard committed
193
#ifdef _DEBUG_
194
195
196
197
198
199
200
  ofstream aof_COMBuffer;
  static unsigned char FirstCall = 1;
  if (FirstCall) {
    aof_COMBuffer.open("CartCOMBuffer_1.dat", ofstream::out);
  } else {
    aof_COMBuffer.open("CartCOMBuffer_1.dat", ofstream::app);
  }
201

202
203
  if (FirstCall)
    FirstCall = 0;
Thomas Moulard's avatar
Thomas Moulard committed
204
205
#endif

206
  for (unsigned int i = 0; i < ZMPRefBuffer.size() - m_NL; i++) {
207

208
    aFIFOZMPRefPositions.push_back(ZMPRefBuffer[i + m_NL]);
209

210
211
    m_PC->OneIterationOfPreview(aPC1x, aPC1y, aSxzmp, aSyzmp,
                                aFIFOZMPRefPositions, 0, aZmpx2, aZmpy2, true);
212

213
214
215
216
    for (unsigned j = 0; j < 3; j++) {
      m_COMBuffer[i].x[j] = aPC1x(j, 0);
      m_COMBuffer[i].y[j] = aPC1y(j, 0);
    }
217

218
    m_COMBuffer[i].yaw = ZMPRefBuffer[i].theta;
219

220
    aFIFOZMPRefPositions.pop_front();
221

Thomas Moulard's avatar
Thomas Moulard committed
222
#ifdef _DEBUG_
223
224
225
226
227
    if (aof_COMBuffer.is_open()) {
      aof_COMBuffer << ZMPRefBuffer[i].time << " " << ZMPRefBuffer[i].px << " "
                    << m_COMBuffer[i].x[0] << " " << m_COMBuffer[i].y[0]
                    << endl;
    }
Thomas Moulard's avatar
Thomas Moulard committed
228
#endif
229
  }
230

Thomas Moulard's avatar
Thomas Moulard committed
231
#ifdef _DEBUG_
232
233
234
  if (aof_COMBuffer.is_open()) {
    aof_COMBuffer.close();
  }
235
#endif
236
}
237

238
void GenerateMotionFromKineoWorks::SetPreviewControl(PreviewControl *aPC) {
Thomas Moulard's avatar
Thomas Moulard committed
239

240
241
242
243
244
245
246
247
  m_PC = aPC;
  m_SamplingPeriod = m_PC->SamplingPeriod();
  m_PreviewControlTime = m_PC->PreviewControlTime();
  if (m_SamplingPeriod == 0)
    m_NL = 0;
  else
    m_NL = (unsigned int)(m_PreviewControlTime / m_SamplingPeriod);
}
248

249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
void GenerateMotionFromKineoWorks::ComputeUpperBodyPosition(
    deque<KWNode> &UpperBodyPositionsBuffer,
    vector<int> &ConversionFromLocalToRobotDOFsIndex) {
  vector<int> ConversionFromLocalToKW;
  int count = 0;
  //! Find the sizes for the buffer, and the conversion array..
  //! First dimension: count the number of DOFs which are -1
  // inside the array of indexes.
  int NbOfUsedDOFs = 0;
  KWNode deltaJoints;

  for (unsigned int i = 0; i < m_IndexFromKWToRobot.size(); i++)
    if (m_IndexFromKWToRobot[i] != -1)
      NbOfUsedDOFs++;
  ConversionFromLocalToRobotDOFsIndex.resize(NbOfUsedDOFs);
  deltaJoints.Joints.resize(NbOfUsedDOFs);
  ConversionFromLocalToKW.resize(NbOfUsedDOFs);

  //! Second dimension: directly the number of elements inside
  // the COM's buffer of position.
  UpperBodyPositionsBuffer.resize(m_COMBuffer.size());
  for (unsigned int i = 0; i < UpperBodyPositionsBuffer.size(); i++) {
    UpperBodyPositionsBuffer[i].Joints.resize(NbOfUsedDOFs);
Thomas Moulard's avatar
Thomas Moulard committed
272
273
  }

274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
  //! First initialize the first upper body position at the
  // beginning of the motion.
  //! and fill the conversion array.
  int k = 0;
  for (unsigned int i = 0; i < m_Path[0].Joints.size(); i++) {
    int IdDOF = 0;
    if ((IdDOF = m_IndexFromKWToRobot[i]) != -1) {

      UpperBodyPositionsBuffer[count].Joints[k] = m_Path[0].Joints[i];
      ConversionFromLocalToRobotDOFsIndex[k] = IdDOF;
      ConversionFromLocalToKW[k] = i;
      k++;
    }
  }
  count++;

  //! For each way-point of the path
  for (unsigned int IdWayPoint = 1; IdWayPoint < m_Path.size(); IdWayPoint++) {
    int CountTarget = -1;
Guilhem Saurel's avatar
Guilhem Saurel committed
293
    double lX = 0.0, lY = 0.0, lZ = 0.0, dist = 1000000.0;
294
295
296
297
298
299
300
301
302

    //! The references are specific to the current hybrid model.
    lX = m_Path[IdWayPoint].Joints[6];
    lY = m_Path[IdWayPoint].Joints[7];
    lZ = m_Path[IdWayPoint].Joints[8];

    //! Find the closest (X,Y,Z) position in the remaining
    // part of the CoM buffer.
    for (unsigned int i = count; i < m_COMBuffer.size(); i++) {
Olivier Stasse's avatar
Olivier Stasse committed
303
      double ldist = (lX - m_COMBuffer[i].x[0]) * (lX - m_COMBuffer[i].x[0]) +
Guilhem Saurel's avatar
Guilhem Saurel committed
304
305
306
                     (lY - m_COMBuffer[i].y[0]) * (lY - m_COMBuffer[i].y[0]) +
                     (lZ - m_COMBuffer[i].z[0]) * (lZ - m_COMBuffer[i].z[0]);

307
308
309
      if (ldist < dist) {
        dist = ldist;
        CountTarget = i;
Thomas Moulard's avatar
Thomas Moulard committed
310
      }
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
    }

    /* cout << "lX: " << lX <<" lY: " << lY << " lZ: " << lZ << endl
       << dist << " " << CountTarget << " " << count << endl; */
    /*! Create the linear interpolation between the previous
      reference value and the newly found. */

    //! Computes the delta for each joint and for each 0.005 ms
    for (unsigned int i = 0; i < ConversionFromLocalToRobotDOFsIndex.size();
         i++) {
      // int j = ConversionFromLocalToRobotDOFsIndex[i];
      int k = ConversionFromLocalToKW[i];

      deltaJoints.Joints[i] =
          (m_Path[IdWayPoint].Joints[k] - m_Path[IdWayPoint - 1].Joints[k]) /
          (double)(CountTarget - count);
    }

    //! Fill the buffer with linear interpolation.
    while (count <= CountTarget) {
      for (unsigned int i = 0; i < ConversionFromLocalToRobotDOFsIndex.size();
           i++) {
        UpperBodyPositionsBuffer[count].Joints[i] =
            UpperBodyPositionsBuffer[count - 1].Joints[i] +
            deltaJoints.Joints[i];
Thomas Moulard's avatar
Thomas Moulard committed
336
      }
337

338
339
340
      count++;
    }
  }
olivier-stasse's avatar
olivier-stasse committed
341
}
342
343

} // namespace PatternGeneratorJRL