GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/next-step-pg-sot.cpp Lines: 0 162 0.0 %
Date: 2023-06-05 08:59:09 Branches: 0 222 0.0 %

Line Branch Exec Source
1
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
 * Copyright Projet JRL-Japan, 2007
3
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4
 *
5
 * File:      NextStepPgSot.h
6
 * Project:   SOT
7
 * Author:    Nicolas Mansard
8
 *
9
 * Version control
10
 * ===============
11
 *
12
 *  $Id$
13
 *
14
 * Description
15
 * ============
16
 *
17
 *
18
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
19
20
#include <dynamic-graph/factory.h>
21
#include <dynamic-graph/pool.h>
22
#include <sot/pattern-generator/exception-pg.h>
23
#include <sot/pattern-generator/next-step-pg-sot.h>
24
25
#include <algorithm>
26
#include <cmath>
27
#include <sot/core/debug.hh>
28
#include <sot/core/macros-signal.hh>
29
30
#define PI 3.1416
31
32
namespace dg = dynamicgraph;
33
namespace dynamicgraph {
34
namespace sot {
35
36
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NextStepPgSot, "NextStepPgSot");
37
38
/* --- CONSTRUCT ----------------------------------------------------- */
39
/* --- CONSTRUCT ----------------------------------------------------- */
40
/* --- CONSTRUCT ----------------------------------------------------- */
41
42
NextStepPgSot::NextStepPgSot(const std::string &name) : NextStep(name) {
43
  sotDEBUGIN(5);
44
  m_StepModificationMode = NextStepPgSot::ADDING_STEP;
45
  m_NextStepTime = -1.0;
46
  m_NbOfFirstSteps = 0;
47
  m_PGI = 0;
48
  sotDEBUGOUT(5);
49
50
  stepbuf.reserve(10000);
51
}
52
53
/* --- FUNCTIONS ------------------------------------------------------- */
54
/* --- FUNCTIONS ------------------------------------------------------- */
55
56
void positionClipper(double x, double y, double &x_result, double &y_result) {
57
  const double MIN_y = 0.16;
58
  const double MAX_y = 0.40;
59
  const double MAX_x = 0.25;
60
  const double EDGE_POINT_x = 0.20;
61
  const double EDGE_POINT_y = 0.30;
62
63
  double x0 = std::abs(x);
64
  double y0 = std::abs(y);
65
  double alpha = y0 / x0;
66
  double y_inter = MIN_y;
67
  double x_inter = MIN_y / alpha;
68
69
  if (alpha < MIN_y / MAX_x) {
70
    x_result = MAX_x;
71
    y_result = MIN_y;
72
  } else if (alpha < EDGE_POINT_y / EDGE_POINT_x) {
73
    double delta1 = (MIN_y - EDGE_POINT_y) / (MAX_x - EDGE_POINT_x);
74
    double x_inter2 = (EDGE_POINT_y - delta1 * EDGE_POINT_x) / (alpha - delta1);
75
    double y_inter2 = alpha * x_inter2;
76
    if (x0 < x_inter) {
77
      x_result = x_inter;
78
      y_result = y_inter;
79
    } else if (x0 > x_inter2) {
80
      x_result = x_inter2;
81
      y_result = y_inter2;
82
    } else {
83
      x_result = x0;
84
      y_result = y0;
85
    }
86
  } else {
87
    double delta2 = (EDGE_POINT_y - MAX_y) / (EDGE_POINT_x);
88
    double x_inter2 = (MAX_y) / (alpha - delta2);
89
    double y_inter2 = alpha * x_inter2;
90
    if (x0 < x_inter) {
91
      x_result = x_inter;
92
      y_result = y_inter;
93
    } else if (x0 > x_inter2) {
94
      x_result = x_inter2;
95
      y_result = y_inter2;
96
    } else {
97
      x_result = x0;
98
      y_result = y0;
99
    }
100
  }
101
102
  if (x < 0) x_result = -x_result;
103
  if (y < 0) y_result = -y_result;
104
}
105
106
/* --- FUNCTIONS -------------------------------------------------- */
107
108
void NextStepPgSot::starter(const int &timeCurr) {
109
  sotDEBUGIN(15);
110
111
  NextStep::starter(timeCurr);
112
113
  if (pgEntity) {
114
    std::ostringstream cmdstd;
115
    cmdstd << ":StartOnLineStepSequencing ";
116
    for (std::deque<FootPrint>::const_iterator iter = footPrintList.begin();
117
         iter != footPrintList.end(); ++iter) {
118
      cmdstd << iter->x << " " << iter->y << " " << iter->theta << " ";
119
    }
120
    std::istringstream cmdArg(cmdstd.str());
121
    std::istringstream emptyArg;
122
    m_sPG->InitState();
123
    m_sPG->pgCommandLine(cmdArg.str());
124
    sotDEBUG(15) << "Cmd: " << cmdstd.str() << std::endl;
125
  } else {
126
    sotERROR << "PG not set" << std::endl;
127
  }
128
129
  state = STATE_STARTED;
130
131
  sotDEBUGOUT(15);
132
  return;
133
}
134
135
void NextStepPgSot::stoper(const int &) {
136
  sotDEBUGIN(15);
137
138
  if (pgEntity) {
139
    std::ostringstream cmdstd;
140
    std::ostringstream os;
141
    cmdstd << ":StopOnLineStepSequencing";
142
    std::istringstream cmdArg(cmdstd.str());
143
    m_sPG->pgCommandLine(cmdArg.str());
144
  } else {
145
    sotERROR << "PG not set" << std::endl;
146
  }
147
148
  m_NbOfFirstSteps = 0;
149
  state = STATE_STOPED;
150
151
  sotDEBUGOUT(15);
152
  return;
153
}
154
155
void NextStepPgSot::introductionCallBack(const int &timeCurr) {
156
  sotDEBUGIN(15);
157
158
  if (state == STATE_STARTED) {
159
    FootPrint &lastStep = footPrintList.back();
160
    if (pgEntity) {
161
      if (m_StepModificationMode == NextStepPgSot::ADDING_STEP) {
162
        std::string cmdLine = "addStep";
163
        std::ostringstream cmdArgIn;
164
        std::ostringstream os;
165
        cmdArgIn << cmdLine << " " << lastStep.x << " " << lastStep.y << " "
166
                 << lastStep.theta;
167
        std::istringstream cmdArg(cmdArgIn.str());
168
        m_sPG->pgCommandLine(cmdArgIn.str());
169
      } else if (m_StepModificationMode == NextStepPgSot::CHANGING_STEP) {
170
        if (m_NbOfFirstSteps < 3)
171
          m_NbOfFirstSteps++;
172
        else {
173
          sotDEBUG(15) << "m_NextStepTime: " << m_NextStepTime << std::endl;
174
175
          PatternGeneratorJRL::FootAbsolutePosition aFAP;
176
177
          int lSupportFoot = 0;
178
179
          lSupportFoot = m_sPG->SupportFootSOUT(timeCurr);
180
181
          double NextFAPx = lastStep.x;
182
          double NextFAPy = lastStep.y;
183
          double NextFAPtheta = lastStep.theta;
184
          if (1) {
185
            NextFAPx = 0.80 * (double)rand() / (double)RAND_MAX - 0.40;
186
            NextFAPy = 0.80 * (double)rand() / (double)RAND_MAX - 0.40;
187
            NextFAPtheta = 18.0 * (double)rand() / (double)RAND_MAX - 6.0;
188
          } else {
189
            NextFAPx = 0.2;
190
            NextFAPy = 0.0;
191
            NextFAPtheta = 45.0;
192
          }
193
194
          /* Warning : HARDCODED VALUES */
195
#if 1
196
          if (0 && mode == MODE_1D) {
197
            const double LIMIT_X = 0.20;
198
            if (NextFAPx < -LIMIT_X) NextFAPx = -LIMIT_X;
199
            if (NextFAPx > LIMIT_X) NextFAPx = LIMIT_X;
200
            NextFAPtheta = 0.;
201
            NextFAPy = 0.;
202
          } else {
203
            // const double LIMIT_X = 0.10;
204
            // const double LIMIT_Y_SUP = 0.05;
205
            // const double LIMIT_Y_INF = 0.03;
206
            const double THETA_MAX = 9.;
207
208
            if (NextFAPtheta < -THETA_MAX) NextFAPtheta = -THETA_MAX;
209
            if (NextFAPtheta > THETA_MAX) NextFAPtheta = THETA_MAX;
210
211
            if (lSupportFoot == 1) {
212
              if (NextFAPy < 0) NextFAPy = -NextFAPy;
213
              double Next_xResult;
214
              double Next_yResult;
215
              positionClipper(NextFAPx, NextFAPy, Next_xResult, Next_yResult);
216
              NextFAPx = cos(NextFAPtheta * PI / 180) * Next_xResult +
217
                         sin(NextFAPtheta * PI / 180) * Next_yResult;
218
              NextFAPy = -sin(NextFAPtheta * PI / 180) * Next_xResult +
219
                         cos(NextFAPtheta * PI / 180) * Next_yResult;
220
221
              // This is the right foot support.
222
              // Make sure this is relative to the
223
              // next stepping value.
224
              std::cout << NextFAPx << " " << NextFAPy << " " << NextFAPtheta
225
                        << std::endl;
226
              NextFAPy = NextFAPy - 0.19;
227
            } else {
228
              if (NextFAPy > 0) NextFAPy = -NextFAPy;
229
              double Next_xResult;
230
              double Next_yResult;
231
              positionClipper(NextFAPx, NextFAPy, Next_xResult, Next_yResult);
232
              NextFAPx = cos(NextFAPtheta * PI / 180) * Next_xResult +
233
                         sin(NextFAPtheta * PI / 180) * Next_yResult;
234
              NextFAPy = -sin(NextFAPtheta * PI / 180) * Next_xResult +
235
                         cos(NextFAPtheta * PI / 180) * Next_yResult;
236
237
              // This is the left foot support.
238
              // Make sure this is relative to the
239
              // next stepping value.
240
              std::cout << NextFAPx << " " << NextFAPy << " " << NextFAPtheta
241
                        << std::endl;
242
              NextFAPy = NextFAPy + 0.19;
243
            }
244
          }
245
#endif
246
          /* End of Warning : HARDCODED VALUES */
247
          aFAP.x = NextFAPx;
248
          aFAP.y = NextFAPy;
249
          aFAP.theta = NextFAPtheta;
250
          // std::cout << lSupportFoot << std::endl;
251
          // std::cout << "FAP = ( "<< aFAP.x << " " << aFAP.y
252
          // << " " << aFAP.z << " ) " << std::endl;
253
          stepbuf.push_back(std::make_pair(lSupportFoot, aFAP));
254
          m_NextStepTime = 0.0;
255
256
          m_PGI->ChangeOnLineStep(0.805, aFAP, m_NextStepTime);
257
258
          if (m_NextStepTime < 0.0)
259
            period = NextStep::PERIOD_DEFAULT;
260
          else
261
            period = (unsigned int)((m_NextStepTime + 0.025) * 200.0);
262
        }
263
      }
264
    } else {
265
      sotERROR << "Walk plugin not found. " << std::endl;
266
      /* TODO Error: PG not set. */
267
    }
268
  }
269
270
  sotDEBUGOUT(15);
271
  return;
272
}
273
274
void NextStepPgSot::commandLine(const std::string &cmdLine,
275
                                std::istringstream &cmdArgs, std::ostream &os) {
276
  if ("help" == cmdLine) {
277
    os << "NextStepPgSot: " << std::endl
278
       << " - initPg [<pg_name>]" << std::endl
279
       << " - stepmodificationmode [add|change]" << std::endl;
280
    NextStep::commandLine(cmdLine, cmdArgs, os);
281
  } else if ("initPg" == cmdLine) {
282
    std::string name = "pg";
283
    cmdArgs >> std::ws;
284
    if (cmdArgs.good()) cmdArgs >> name;
285
    pgEntity = &(PoolStorage::getInstance()->getEntity(name));
286
    m_sPG = dynamic_cast<PatternGenerator *>(pgEntity);
287
    if (m_sPG != 0) m_PGI = m_sPG->GetPatternGeneratorInterface();
288
  } else if ("stepmodificationmode" == cmdLine) {
289
    std::string stgmode;
290
    cmdArgs >> std::ws;
291
    if (cmdArgs.good()) {
292
      cmdArgs >> stgmode;
293
      if (stgmode == "add")
294
        m_StepModificationMode = NextStepPgSot::ADDING_STEP;
295
      else if (stgmode == "change")
296
        m_StepModificationMode = NextStepPgSot::CHANGING_STEP;
297
    } else {
298
      if (m_StepModificationMode == NextStepPgSot::ADDING_STEP)
299
        os << "add";
300
      else if (m_StepModificationMode == NextStepPgSot::CHANGING_STEP)
301
        os << "change";
302
    }
303
  } else if ("savesteps" == cmdLine) {
304
    std::ofstream os("/tmp/steps.dat");
305
    for (size_t i = 0; i < stepbuf.size(); ++i) {
306
      os << stepbuf[i].first << " " << stepbuf[i].second.x << " "
307
         << stepbuf[i].second.y << " " << stepbuf[i].second.z << "\n";
308
    }
309
    os << std::endl;
310
    stepbuf.clear();
311
  } else {
312
    NextStep::commandLine(cmdLine, cmdArgs, os);
313
  }
314
}
315
316
}  // namespace sot
317
}  // namespace dynamicgraph