pgtypes.hh 8.85 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 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
21
22
23
 *
 * Mehdi    Benallegue
 * Andrei   Herdt
 * Francois Keith
 * 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/>.
 *
24
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
25
26
27
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PGTypes.h
Olivier Stasse's avatar
Olivier Stasse committed
28
  \brief Defines basic types for the Humanoid Walking Pattern Generator.
Thomas Moulard's avatar
Thomas Moulard committed
29
30
*/
#ifndef _PATTERN_GENERATOR_TYPES_H_
31
#define _PATTERN_GENERATOR_TYPES_H_
Thomas Moulard's avatar
Thomas Moulard committed
32
33

// For Windows compatibility.
34
35
36
#if defined(WIN32)
#ifdef jrl_walkgen_EXPORTS
#define WALK_GEN_JRL_EXPORT __declspec(dllexport)
Thomas Moulard's avatar
Thomas Moulard committed
37
#else
38
39
40
41
#define WALK_GEN_JRL_EXPORT __declspec(dllimport)
#endif
#else
#define WALK_GEN_JRL_EXPORT
Thomas Moulard's avatar
Thomas Moulard committed
42
#endif
olivier stasse's avatar
olivier stasse committed
43
#include <fstream>
44
#include <iostream>
45
46
47

#include <Eigen/Dense>
#include <vector>
Thomas Moulard's avatar
Thomas Moulard committed
48

49
50
namespace PatternGeneratorJRL {
struct COMState_s;
Thomas Moulard's avatar
Thomas Moulard committed
51

52
53
54
55
56
57
58
/// Structure to store the COM position computed by the preview control.
struct WALK_GEN_JRL_EXPORT COMPosition_s {
  double x[3], y[3];
  double z[3];
  double yaw;   // aka theta
  double pitch; // aka omega
  double roll;  // aka hip
59

60
61
  struct COMPosition_s &operator=(const COMState_s &aCS);
};
Thomas Moulard's avatar
Thomas Moulard committed
62

63
64
65
66
inline std::ostream &operator<<(std::ostream &os, const COMPosition_s &aCp) {
  for (size_t i = 0; i < 3; ++i) {
    os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i]
       << " z[" << i << "] " << aCp.z[i] << std::endl;
67
  }
68
69
70
  os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
  return os;
}
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
typedef struct COMPosition_s COMPosition;
typedef struct COMPosition_s WaistState;

/// Structure to store the COM state computed by the preview control.
struct WALK_GEN_JRL_EXPORT COMState_s {
  double x[3], y[3], z[3];
  double yaw[3];   // aka theta
  double pitch[3]; // aka omega
  double roll[3];  // aka hip

  struct COMState_s &operator=(const COMPosition_s &aCS);

  void reset();

  COMState_s();

  friend std::ostream &operator<<(std::ostream &os,
                                  const struct COMState_s &acs);
};

typedef struct COMState_s COMState;

/** Structure to store each foot position when the user is specifying
    a sequence of relative positions. */
struct RelativeFootPosition_s {
  double sx, sy, sz, theta;
  double SStime;
  double DStime;
  int stepType; // 1:normal walking 2:one step before obstacle
                // 3:first leg over obstacle 4:second leg over obstacle
  // 5:one step after obstacle 6 :stepping stair
  double DeviationHipHeight;
  RelativeFootPosition_s();
};
typedef struct RelativeFootPosition_s RelativeFootPosition;

inline std::ostream &operator<<(std::ostream &os,
                                const RelativeFootPosition_s &rfp) {
  os << "sx " << rfp.sx << " sy " << rfp.sy << " sz " << rfp.sz << " theta "
     << rfp.theta << std::endl;
  os << "SStime " << rfp.SStime << " DStime " << rfp.DStime << " stepType "
     << rfp.stepType << " DeviationHipHeight " << rfp.DeviationHipHeight;
  return os;
}
116

117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
/** Structure to store each of the ZMP value, with a given
    direction at a certain time. */
struct ZMPPosition_s {
  double px, py, pz;
  double theta; // For COM
  double time;
  int stepType; // 1:normal walking 2:one step before obstacle
                // 3:first leg over obstacle 4:second leg over
  // obstacle 5:one step after obstacle
  // +10 if double support phase
  // *(-1) if right foot stance else left foot stance
};
typedef struct ZMPPosition_s ZMPPosition;

inline std::ostream &operator<<(std::ostream &os, const ZMPPosition_s &zmp) {
  os << "px " << zmp.px << " py " << zmp.pz << " pz " << zmp.pz << " theta "
     << zmp.theta << std::endl;
  os << "time " << zmp.time << " stepType " << zmp.stepType;
  return os;
}
137

138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
/// Structure to store the absolute foot position.
struct FootAbsolutePosition_t {
  /*! px, py in meters, theta in DEGREES. */
  double x, y, z, theta, omega, omega2;
  /*! Speed of the foot. */
  double dx, dy, dz, dtheta, domega, domega2;
  /*! Acceleration of the foot. */
  double ddx, ddy, ddz, ddtheta, ddomega, ddomega2;
  /*! Jerk of the foot. */
  double dddx, dddy, dddz, dddtheta, dddomega, dddomega2;
  /*! Time at which this position should be reached. */
  double time;
  /*! 1:normal walking 2:one step before obstacle
    3:first leg over obstacle 4:second leg over obstacle
    5:one step after  obstacle
    +10 if double support phase
    (-1) if support foot  */
  int stepType;
};
typedef struct FootAbsolutePosition_t FootAbsolutePosition;

inline std::ostream &operator<<(std::ostream &os,
                                const FootAbsolutePosition &fap) {
  os << "x " << fap.x << " y " << fap.y << " z " << fap.z << " theta "
     << fap.theta << " omega " << fap.omega << " omega2 " << fap.omega2
     << std::endl;
  os << "dx " << fap.dx << " dy " << fap.dy << " dz " << fap.dz << " dtheta "
     << fap.dtheta << " domega " << fap.domega << " domega2 " << fap.domega2
     << std::endl;
  os << "ddx " << fap.ddx << " ddy " << fap.ddy << " ddz " << fap.ddz
     << " ddtheta " << fap.ddtheta << " ddomega " << fap.ddomega << " ddomega2 "
     << fap.ddomega2 << std::endl;
  os << "time " << fap.time << " stepType " << fap.stepType;
  return os;
}
173

174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
/// Structure to store the absolute foot position.
struct HandAbsolutePosition_t {
  /*! x, y, z in meters, theta in DEGREES. */
  double x, y, z, theta, omega, omega2;
  /*! Speed of the foot. */
  double dx, dy, dz, dtheta, domega, domega2;
  /*! Acceleration of the foot. */
  double ddx, ddy, ddz, ddtheta, ddomega, ddomega2;
  /*! Jerk of the hand. */
  double dddx, dddy, dddz, dddtheta, dddomega, dddomega2;
  /*! Time at which this position should be reached. */
  double time;
  /*! -1 : contact
   *   1 : no contact
   */
  int stepType;
};
typedef struct HandAbsolutePosition_t HandAbsolutePosition;

inline std::ostream &operator<<(std::ostream &os,
                                const HandAbsolutePosition &hap) {
  os << "x " << hap.x << " y " << hap.y << " z " << hap.z << " theta "
     << hap.theta << " omega " << hap.omega << " omega2 " << hap.omega2
     << std::endl;
  os << "dx " << hap.dx << " dy " << hap.dy << " dz " << hap.dz << " dtheta "
     << hap.dtheta << " domega " << hap.domega << " domega2 " << hap.domega2
     << std::endl;
  os << "ddx " << hap.ddx << " ddy " << hap.ddy << " ddz " << hap.ddz
     << " ddtheta " << hap.ddtheta << " ddomega " << hap.ddomega << " ddomega2 "
     << hap.ddomega2 << std::endl;
  os << "time " << hap.time << " stepType " << hap.stepType;
  return os;
}
207

208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
// Linear constraint.
struct LinearConstraintInequality_s {
  Eigen::MatrixXd A;
  Eigen::MatrixXd B;
  Eigen::VectorXd Center;
  std::vector<int> SimilarConstraints;
  double StartingTime, EndingTime;
};
typedef struct LinearConstraintInequality_s LinearConstraintInequality_t;

/// Linear constraints with variable feet placement.
struct LinearConstraintInequalityFreeFeet_s {
  Eigen::MatrixXd D;
  Eigen::MatrixXd Dc;
  int StepNumber;
};
typedef struct LinearConstraintInequalityFreeFeet_s
225
    LinearConstraintInequalityFreeFeet_t;
Thomas Moulard's avatar
Thomas Moulard committed
226

227
228
229
230
231
232
233
234
235
236
237
238
// State of the feet on the ground
struct SupportFeet_s {
  double x, y, theta, StartTime;
  int SupportFoot;
};
typedef struct SupportFeet_s SupportFeet_t;

inline std::ostream &operator<<(std::ostream &os, const SupportFeet_s &sf) {
  os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
  os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
  return os;
}
239

240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
/// Structure to store the absolute reference.
struct ReferenceAbsoluteVelocity_t {
  /*! m/sec or degrees/sec */
  double x, y, z, dYaw;

  /*! reference values for the whole preview window */
  Eigen::VectorXd RefVectorX;
  Eigen::VectorXd RefVectorY;
  Eigen::VectorXd RefVectorTheta;
};
typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;

inline std::ostream &operator<<(std::ostream &os,
                                const ReferenceAbsoluteVelocity_t &rav) {
  os << "x " << rav.x << " y " << rav.y << " z " << rav.z << " dYaw "
     << rav.dYaw;
  return os;
}
mnaveau's avatar
mnaveau committed
258

259
260
261
262
263
264
265
266
267
268
269
270
/// Structure to model a circle (e.g : a stricly convex obstable)
struct Circle_t {
  double x_0;
  double y_0;
  double r;
  double margin;
};
typedef struct Circle_t Circle;

inline std::ostream &operator<<(std::ostream &os, const Circle_t &circle) {
  os << "x_0 " << circle.x_0 << " y_0 " << circle.y_0 << " R " << circle.r;
  return os;
olivier-stasse's avatar
olivier-stasse committed
271
}
272
273

} // namespace PatternGeneratorJRL
Thomas Moulard's avatar
Thomas Moulard committed
274
#endif