privatepgtypes.cpp 6.52 KB
Newer Older
1
/*
2
 * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
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/>.
 *
22
 *  Research carried out within the scope of the
23
24
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
25
#include <privatepgtypes.hh>
26

27
#include <fstream>
28
#include <iostream>
29

30
namespace PatternGeneratorJRL {
Andrei's avatar
Andrei committed
31

32
struct support_state_t &support_state_t::operator=(const support_state_t &aSS) {
Andrei's avatar
Andrei committed
33

34
35
36
37
38
39
40
41
42
43
  Phase = aSS.Phase;
  Foot = aSS.Foot;
  NbStepsLeft = aSS.NbStepsLeft;
  TimeLimit = aSS.TimeLimit;
  StepNumber = aSS.StepNumber;
  StateChanged = aSS.StateChanged;
  StartTime = aSS.StartTime;
  X = aSS.X;
  Y = aSS.Y;
  Yaw = aSS.Yaw;
44

45
46
  return *this;
}
47

48
49
50
51
52
53
54
55
56
57
58
59
60
61
void support_state_t::reset() {

  Phase = DS;
  Foot = LEFT;
  NbStepsLeft = 0;
  TimeLimit = 0.0;
  StepNumber = 0;
  StateChanged = false;
  NbInstants = 0;
  StartTime = 0.0;
  X = 0.0;
  Y = 0.0;
  Yaw = 0.0;
}
62

63
support_state_t::support_state_t() { reset(); }
Andrei's avatar
Andrei committed
64

65
struct com_t &com_t::operator=(const com_t &aCS) {
66

67
68
69
70
71
72
73
  for (unsigned int i = 0; i < 3; i++) {
    x[i] = aCS.x[i];
    y[i] = aCS.y[i];
    z[i] = aCS.z[i];
  };
  return *this;
}
Andrei's avatar
Andrei committed
74

75
void com_t::reset() {
76

77
78
79
80
81
82
83
  x.resize(3);
  y.resize(3);
  z.resize(3);
  x.setZero();
  y.setZero();
  z.setZero();
}
Andrei Herdt's avatar
Andrei Herdt committed
84

85
com_t::com_t() { reset(); }
86

87
struct trunk_t &trunk_t::operator=(const trunk_t &aTS) {
Andrei's avatar
Andrei committed
88

89
90
91
92
  for (unsigned int i = 0; i < 3; i++) {
    x[i] = aTS.x[i];
    y[i] = aTS.y[i];
    z[i] = aTS.z[i];
Andrei's avatar
Andrei committed
93

94
95
96
97
98
99
    yaw[i] = aTS.yaw[i];
    pitch[i] = aTS.pitch[i];
    roll[i] = aTS.roll[i];
  };
  return *this;
}
Andrei Herdt's avatar
Andrei Herdt committed
100

101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
void trunk_t::reset() {

  x.resize(3);
  y.resize(3);
  z.resize(3);
  yaw.resize(3);
  pitch.resize(3);
  roll.resize(3);
  x.setZero();
  y.setZero();
  z.setZero();
  yaw.setZero();
  pitch.setZero();
  roll.setZero();
}
Andrei's avatar
Andrei committed
116

117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
trunk_t::trunk_t() { reset(); }

void convex_hull_t::rotate(axis_e axis, double angle) {

  switch (axis) {
  case YAW:
    double xOld, yOld;
    for (unsigned int j = 0; j < X_vec.size(); j++) {
      xOld = X_vec[j];
      yOld = Y_vec[j];
      X_vec[j] = (xOld * cos(angle) - yOld * sin(angle));
      Y_vec[j] = (xOld * sin(angle) + yOld * cos(angle));
    }
    break;
  case PITCH:
    break;
  case ROLL:
    break;
  case X_AXIS:
    break;
  case Y_AXIS:
    break;
  case Z_AXIS:
    break;
141
  }
142
}
143

144
145
146
147
148
convex_hull_t::convex_hull_t(unsigned nbVert, unsigned nbIneq)
    : nbIneq_(0), nbVert_(0) {
  clear();
  resize(nbVert, nbIneq);
}
Andrei's avatar
Andrei committed
149

150
void convex_hull_t::clear() {
Andrei's avatar
Andrei committed
151

152
153
154
155
156
157
158
159
  X_vec.clear();
  Y_vec.clear();
  Z_vec.clear();
  A_vec.clear();
  B_vec.clear();
  C_vec.clear();
  D_vec.clear();
}
160

161
void convex_hull_t::resize(unsigned nbVert, unsigned nbIneq) {
Andrei's avatar
Andrei committed
162

163
164
165
166
167
168
169
  X_vec.resize(nbVert);
  Y_vec.resize(nbVert);
  Z_vec.resize(nbVert);
  A_vec.resize(nbIneq);
  B_vec.resize(nbIneq);
  C_vec.resize(nbIneq);
  D_vec.resize(nbIneq);
Andrei Herdt's avatar
Andrei Herdt committed
170

171
172
173
  nbVert_ = nbVert;
  nbIneq_ = nbIneq;
}
Andrei Herdt's avatar
Andrei Herdt committed
174

175
176
void convex_hull_t::set_vertices(const double *X_a, const double *Y_a,
                                 const double *Z_a) {
Andrei Herdt's avatar
Andrei Herdt committed
177

178
179
180
181
  for (unsigned i = 0; i < nbVert_; i++) {
    X_vec[i] = X_a[i];
    Y_vec[i] = Y_a[i];
    Z_vec[i] = Z_a[i];
Andrei Herdt's avatar
Andrei Herdt committed
182
  }
183
184
}
void convex_hull_t::set_vertices(const double *X_a, const double *Y_a) {
Andrei Herdt's avatar
Andrei Herdt committed
185

186
187
188
  for (unsigned i = 0; i < nbVert_; i++) {
    X_vec[i] = X_a[i];
    Y_vec[i] = Y_a[i];
Andrei's avatar
Andrei committed
189
  }
190
191
192
193
194
195
196
197
198
}
void convex_hull_t::set_inequalities(const double *A_a, const double *B_a,
                                     const double *C_a, const double *D_a) {

  for (unsigned i = 0; i < nbIneq_; i++) {
    A_vec[i] = A_a[i];
    B_vec[i] = B_a[i];
    C_vec[i] = C_a[i];
    D_vec[i] = D_a[i];
Andrei's avatar
Andrei committed
199
  }
200
}
Andrei's avatar
Andrei committed
201

202
void convex_hull_t::cout() {
203

204
205
206
207
  std::cout << "Vertices: " << nbVert_ << std::endl;
  for (unsigned i = 0; i < nbVert_; ++i)
    std::cout << X_vec[i] << " " << Y_vec[i] << std::endl;
  std::cout << std::endl;
Andrei Herdt's avatar
Andrei Herdt committed
208

209
210
211
212
213
214
  std::cout << "Inequalities: " << nbIneq_ << std::endl;
  for (unsigned i = 0; i < nbIneq_; ++i)
    std::cout << A_vec[i] << " " << B_vec[i] << " " << C_vec[i] << " "
              << D_vec[i] << std::endl;
  std::cout << std::endl;
}
Andrei Herdt's avatar
Andrei Herdt committed
215

216
void linear_inequality_t::clear() {
Andrei Herdt's avatar
Andrei Herdt committed
217

218
219
220
221
222
  D.X_mat.setZero();
  D.Y_mat.setZero();
  D.Z_mat.setZero();
  Dc_vec.setZero();
}
Andrei Herdt's avatar
Andrei Herdt committed
223

Guilhem Saurel's avatar
Guilhem Saurel committed
224
void linear_inequality_t::resize(int NbRows, int NbCols, bool) {
Andrei Herdt's avatar
Andrei Herdt committed
225

226
227
228
229
230
  D.X_mat.resize(NbRows, NbCols);
  D.Y_mat.resize(NbRows, NbCols);
  D.Z_mat.resize(NbRows, NbCols);
  Dc_vec.resize(NbRows);
}
231

232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
solution_t::solution_t()
    : NbVariables(0), NbConstraints(0), Fail(0), Print(0), Solution_vec(0),
      SupportOrientations_deq(0), SupportStates_deq(0), ConstrLagr_vec(0),
      LBoundsLagr_vec(0), UBoundsLagr_vec(0) {}

void solution_t::reset() {

  NbVariables = 0;
  NbConstraints = 0;
  Fail = 0;
  Print = 0;

  Solution_vec.resize(0, false);
  SupportOrientations_deq.resize(0);
  TrunkOrientations_deq.resize(0);
  SupportStates_deq.resize(0);
  ConstrLagr_vec.resize(0, false);
  LBoundsLagr_vec.resize(0, false);
  UBoundsLagr_vec.resize(0, false);
}
252

253
254
255
256
void solution_t::resize(unsigned int SizeSolution,
                        unsigned int SizeConstraints) {
  NbVariables = SizeSolution;
  NbConstraints = SizeConstraints;
257

258
259
260
261
262
  Solution_vec.resize(SizeSolution, false);
  ConstrLagr_vec.resize(SizeConstraints, false);
  LBoundsLagr_vec.resize(SizeSolution, false);
  UBoundsLagr_vec.resize(SizeSolution, false);
}
263

264
265
266
267
268
269
void solution_t::dump(const char *FileName) {
  std::ofstream aof;
  aof.open(FileName, std::ofstream::out);
  print(aof);
  aof.close();
}
270

271
272
273
274
275
276
277
void solution_t::print(std::ostream &aos) {
  aos << "Arrays:" << std::endl << "Solution: ";
  for (unsigned int i = 0; i < NbVariables; i++) {
    aos << Solution_vec[i] << " ";
  };
  aos << std::endl;
}
278

279
reference_t::reference_t() : Global(), Local() {}
280

281
282
reference_t::reference_t(const reference_t &R)
    : Global(R.Global), Local(R.Local) {}
283

284
285
reference_t::frame_t::frame_t()
    : X(0), Y(0), Yaw(0), X_vec(), Y_vec(), Yaw_vec() {}
286

287
288
289
reference_t::frame_t::frame_t(const frame_t &F)
    : X(F.X), Y(F.Y), Yaw(F.Yaw), X_vec(F.X_vec), Y_vec(F.Y_vec),
      Yaw_vec(F.Yaw_vec) {}
290

291
} // namespace PatternGeneratorJRL