pgtypes.hh 8.94 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
31
32
33
34
*/
#ifndef _PATTERN_GENERATOR_TYPES_H_
#define  _PATTERN_GENERATOR_TYPES_H_

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

namespace PatternGeneratorJRL
{
  struct COMState_s;

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

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

Thomas Moulard's avatar
Thomas Moulard committed
62
63
  };

64
65
66
67
68
69
70
71
72
73
  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;
    }
    os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
    return os;
  }

Thomas Moulard's avatar
Thomas Moulard committed
74
75
76
77
78
79
80
81
82
83
  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
84

Thomas Moulard's avatar
Thomas Moulard committed
85
    struct COMState_s & operator=(const COMPosition_s &aCS);
86
87

    void reset();
Thomas Moulard's avatar
Thomas Moulard committed
88
89

    COMState_s();
olivier stasse's avatar
olivier stasse committed
90
91

    friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
Thomas Moulard's avatar
Thomas Moulard committed
92
  };
93
94


Thomas Moulard's avatar
Thomas Moulard committed
95
96

  typedef struct COMState_s COMState;
97
98

  /** Structure to store each foot position when the user is specifying
Thomas Moulard's avatar
Thomas Moulard committed
99
100
      a sequence of relative positions. */
  struct RelativeFootPosition_s
101
  {
student's avatar
student committed
102
    double sx,sy,sz,theta;
olivier-stasse's avatar
olivier-stasse committed
103
104
    double SStime;
    double DStime;
Francois Keith's avatar
Francois Keith committed
105
    int stepType;     //1:normal walking 2:one step before obstacle
student's avatar
student committed
106
                      //3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle 6 :stepping stair
107
    double DeviationHipHeight;
Olivier Stasse's avatar
Olivier Stasse committed
108
  };
Thomas Moulard's avatar
Thomas Moulard committed
109
110
  typedef struct RelativeFootPosition_s RelativeFootPosition;

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

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

132
133
134
135
136
137
138
  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;
  }

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

160
161
162
163
164
165
166
167
168
  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;
  }

169
170
171
172
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
  /// 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;
  }

Thomas Moulard's avatar
Thomas Moulard committed
198
199
200
201
202
203
204
205
206
  // Linear constraint.
  struct LinearConstraintInequality_s
  {
    MAL_MATRIX(A,double);
    MAL_MATRIX(B,double);
    MAL_VECTOR(Center,double);
    std::vector<int> SimilarConstraints;
    double StartingTime, EndingTime;
  };
207
  typedef struct LinearConstraintInequality_s
Thomas Moulard's avatar
Thomas Moulard committed
208
    LinearConstraintInequality_t;
209

210
211
212
213
214
215
216
217
218
  /// Linear constraints with variable feet placement.
  struct LinearConstraintInequalityFreeFeet_s
  {
    MAL_MATRIX(D,double);
    MAL_MATRIX(Dc,double);
    int StepNumber;
  };
  typedef struct LinearConstraintInequalityFreeFeet_s
    LinearConstraintInequalityFreeFeet_t;
Thomas Moulard's avatar
Thomas Moulard committed
219

220
221
222
223
224
225
226
227
228
  //State of the feet on the ground
  struct SupportFeet_s
  {
    double x,y,theta,StartTime;
    int SupportFoot;
  };
  typedef struct SupportFeet_s
    SupportFeet_t;

229
230
231
232
233
234
235
  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;
  }

olivier-stasse's avatar
olivier-stasse committed
236
  /// Structure to store the absolute reference.
237
238
  struct ReferenceAbsoluteVelocity_t
  {
olivier-stasse's avatar
olivier-stasse committed
239
240
    /*! m/sec or degrees/sec */
    double x,y,z, dYaw;
241

olivier-stasse's avatar
olivier-stasse committed
242
243
244
245
    /*! reference values for the whole preview window */
    MAL_VECTOR(RefVectorX,double);
    MAL_VECTOR(RefVectorY,double);
    MAL_VECTOR(RefVectorTheta,double);
246
247
248
  };
  typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;

249
250
251
252
253
254
  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;
  }

255
256
257
258
259
260
261
262
263
264
  /// 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 ;

mnaveau's avatar
mnaveau committed
265
266
267
268
269
270
  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
}
Thomas Moulard's avatar
Thomas Moulard committed
272
#endif