pgtypes.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2007, 2008, 2009, 2010,
3  *
4  * Mehdi Benallegue
5  * Andrei Herdt
6  * Francois Keith
7  * Olivier Stasse
8  *
9  * JRL, CNRS/AIST
10  *
11  * This file is part of walkGenJrl.
12  * walkGenJrl is free software: you can redistribute it and/or modify
13  * it under the terms of the GNU Lesser General Public License as published by
14  * the Free Software Foundation, either version 3 of the License, or
15  * (at your option) any later version.
16  *
17  * walkGenJrl is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU General Lesser Public License for more details.
21  * You should have received a copy of the GNU Lesser General Public License
22  * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
23  *
24  * Research carried out within the scope of the
25  * Joint Japanese-French Robotics Laboratory (JRL)
26  */
30 #ifndef _PATTERN_GENERATOR_TYPES_H_
31 #define _PATTERN_GENERATOR_TYPES_H_
32 
33 // For Windows compatibility.
34 #if defined(WIN32)
35 #ifdef jrl_walkgen_EXPORTS
36 #define WALK_GEN_JRL_EXPORT __declspec(dllexport)
37 #else
38 #define WALK_GEN_JRL_EXPORT __declspec(dllimport)
39 #endif
40 #else
41 #define WALK_GEN_JRL_EXPORT
42 #endif
43 #include <fstream>
44 #include <iostream>
45 
46 #include <Eigen/Dense>
47 #include <vector>
48 
49 namespace PatternGeneratorJRL {
50 struct COMState_s;
51 
54  double x[3], y[3];
55  double z[3];
56  double yaw; // aka theta
57  double pitch; // aka omega
58  double roll; // aka hip
59 
60  struct COMPosition_s &operator=(const COMState_s &aCS);
61 };
62 
63 inline std::ostream &operator<<(std::ostream &os, const COMPosition_s &aCp) {
64  for (size_t i = 0; i < 3; ++i) {
65  os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i]
66  << " z[" << i << "] " << aCp.z[i] << std::endl;
67  }
68  os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
69  return os;
70 }
71 
72 typedef struct COMPosition_s COMPosition;
73 typedef struct COMPosition_s WaistState;
74 
77  double x[3], y[3], z[3];
78  double yaw[3]; // aka theta
79  double pitch[3]; // aka omega
80  double roll[3]; // aka hip
81 
82  struct COMState_s &operator=(const COMPosition_s &aCS);
83 
84  void reset();
85 
86  COMState_s();
87 
88  friend std::ostream &operator<<(std::ostream &os,
89  const struct COMState_s &acs);
90 };
91 
92 typedef struct COMState_s COMState;
93 
97  double sx, sy, sz, theta;
98  double SStime;
99  double DStime;
100  int stepType; // 1:normal walking 2:one step before obstacle
101  // 3:first leg over obstacle 4:second leg over obstacle
102  // 5:one step after obstacle 6 :stepping stair
105 };
107 
108 inline std::ostream &operator<<(std::ostream &os,
109  const RelativeFootPosition_s &rfp) {
110  os << "sx " << rfp.sx << " sy " << rfp.sy << " sz " << rfp.sz << " theta "
111  << rfp.theta << std::endl;
112  os << "SStime " << rfp.SStime << " DStime " << rfp.DStime << " stepType "
113  << rfp.stepType << " DeviationHipHeight " << rfp.DeviationHipHeight;
114  return os;
115 }
116 
120  double px, py, pz;
121  double theta; // For COM
122  double time;
123  int stepType; // 1:normal walking 2:one step before obstacle
124  // 3:first leg over obstacle 4:second leg over
125  // obstacle 5:one step after obstacle
126  // +10 if double support phase
127  // *(-1) if right foot stance else left foot stance
128 };
129 typedef struct ZMPPosition_s ZMPPosition;
130 
131 inline std::ostream &operator<<(std::ostream &os, const ZMPPosition_s &zmp) {
132  os << "px " << zmp.px << " py " << zmp.pz << " pz " << zmp.pz << " theta "
133  << zmp.theta << std::endl;
134  os << "time " << zmp.time << " stepType " << zmp.stepType;
135  return os;
136 }
137 
141  double x, y, z, theta, omega, omega2;
143  double dx, dy, dz, dtheta, domega, domega2;
149  double time;
155  int stepType;
156 };
158 
159 inline std::ostream &operator<<(std::ostream &os,
160  const FootAbsolutePosition &fap) {
161  os << "x " << fap.x << " y " << fap.y << " z " << fap.z << " theta "
162  << fap.theta << " omega " << fap.omega << " omega2 " << fap.omega2
163  << std::endl;
164  os << "dx " << fap.dx << " dy " << fap.dy << " dz " << fap.dz << " dtheta "
165  << fap.dtheta << " domega " << fap.domega << " domega2 " << fap.domega2
166  << std::endl;
167  os << "ddx " << fap.ddx << " ddy " << fap.ddy << " ddz " << fap.ddz
168  << " ddtheta " << fap.ddtheta << " ddomega " << fap.ddomega << " ddomega2 "
169  << fap.ddomega2 << std::endl;
170  os << "time " << fap.time << " stepType " << fap.stepType;
171  return os;
172 }
173 
177  double x, y, z, theta, omega, omega2;
179  double dx, dy, dz, dtheta, domega, domega2;
185  double time;
189  int stepType;
190 };
192 
193 inline std::ostream &operator<<(std::ostream &os,
194  const HandAbsolutePosition &hap) {
195  os << "x " << hap.x << " y " << hap.y << " z " << hap.z << " theta "
196  << hap.theta << " omega " << hap.omega << " omega2 " << hap.omega2
197  << std::endl;
198  os << "dx " << hap.dx << " dy " << hap.dy << " dz " << hap.dz << " dtheta "
199  << hap.dtheta << " domega " << hap.domega << " domega2 " << hap.domega2
200  << std::endl;
201  os << "ddx " << hap.ddx << " ddy " << hap.ddy << " ddz " << hap.ddz
202  << " ddtheta " << hap.ddtheta << " ddomega " << hap.ddomega << " ddomega2 "
203  << hap.ddomega2 << std::endl;
204  os << "time " << hap.time << " stepType " << hap.stepType;
205  return os;
206 }
207 
208 // Linear constraint.
210  Eigen::MatrixXd A;
211  Eigen::MatrixXd B;
212  Eigen::VectorXd Center;
213  std::vector<int> SimilarConstraints;
215 };
217 
220  Eigen::MatrixXd D;
221  Eigen::MatrixXd Dc;
223 };
226 
227 // State of the feet on the ground
229  double x, y, theta, StartTime;
231 };
233 
234 inline std::ostream &operator<<(std::ostream &os, const SupportFeet_s &sf) {
235  os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
236  os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
237  return os;
238 }
239 
243  double x, y, z, dYaw;
244 
246  Eigen::VectorXd RefVectorX;
247  Eigen::VectorXd RefVectorY;
248  Eigen::VectorXd RefVectorTheta;
249 };
251 
252 inline std::ostream &operator<<(std::ostream &os,
253  const ReferenceAbsoluteVelocity_t &rav) {
254  os << "x " << rav.x << " y " << rav.y << " z " << rav.z << " dYaw "
255  << rav.dYaw;
256  return os;
257 }
258 
260 struct Circle_t {
261  double x_0;
262  double y_0;
263  double r;
264  double margin;
265 };
266 typedef struct Circle_t Circle;
267 
268 inline std::ostream &operator<<(std::ostream &os, const Circle_t &circle) {
269  os << "x_0 " << circle.x_0 << " y_0 " << circle.y_0 << " R " << circle.r;
270  return os;
271 }
272 
274  Eigen::VectorXd CurrentConfiguration;
275  Eigen::VectorXd CurrentVelocity;
276  Eigen::VectorXd CurrentAcceleration;
277  Eigen::VectorXd ZMPTarget;
281  Eigen::VectorXd Momentum;
282 };
283 
284 } // namespace PatternGeneratorJRL
285 #endif
PatternGeneratorJRL::RelativeFootPosition_s::RelativeFootPosition_s
RelativeFootPosition_s()
PatternGeneratorJRL::RelativeFootPosition_s::SStime
double SStime
Definition: pgtypes.hh:98
PatternGeneratorJRL::SupportFeet_s::y
double y
Definition: pgtypes.hh:229
PatternGeneratorJRL::HandAbsolutePosition_t::dz
double dz
Definition: pgtypes.hh:179
PatternGeneratorJRL::HandAbsolutePosition_t::dddx
double dddx
Definition: pgtypes.hh:183
PatternGeneratorJRL::HandAbsolutePosition_t::dddtheta
double dddtheta
Definition: pgtypes.hh:183
PatternGeneratorJRL::SupportFeet_s::StartTime
double StartTime
Definition: pgtypes.hh:229
PatternGeneratorJRL::FootAbsolutePosition_t::dddomega2
double dddomega2
Definition: pgtypes.hh:147
PatternGeneratorJRL::HandAbsolutePosition_t::dtheta
double dtheta
Definition: pgtypes.hh:179
PatternGeneratorJRL::HandAbsolutePosition_t::ddy
double ddy
Definition: pgtypes.hh:181
PatternGeneratorJRL::HandAbsolutePosition_t::domega2
double domega2
Definition: pgtypes.hh:179
PatternGeneratorJRL::FootAbsolutePosition_t::stepType
int stepType
Definition: pgtypes.hh:155
PatternGeneratorJRL::HandAbsolutePosition_t::z
double z
Definition: pgtypes.hh:177
PatternGeneratorJRL::FootAbsolutePosition_t::omega
double omega
Definition: pgtypes.hh:141
PatternGeneratorJRL::LinearConstraintInequality_s::B
Eigen::MatrixXd B
Definition: pgtypes.hh:211
PatternGeneratorJRL::RelativeFootPosition_s::stepType
int stepType
Definition: pgtypes.hh:100
PatternGeneratorJRL::ZMPPosition_s::px
double px
Definition: pgtypes.hh:120
PatternGeneratorJRL::ZMPPosition_s::theta
double theta
Definition: pgtypes.hh:121
PatternGeneratorJRL::LinearConstraintInequality_s::SimilarConstraints
std::vector< int > SimilarConstraints
Definition: pgtypes.hh:213
PatternGeneratorJRL::FootAbsolutePosition_t::ddx
double ddx
Definition: pgtypes.hh:145
PatternGeneratorJRL::FootAbsolutePosition_t::time
double time
Definition: pgtypes.hh:149
PatternGeneratorJRL::FootAbsolutePosition_t::ddz
double ddz
Definition: pgtypes.hh:145
PatternGeneratorJRL::ZMPPosition_s
Definition: pgtypes.hh:119
PatternGeneratorJRL::FootAbsolutePosition_t::x
double x
Definition: pgtypes.hh:141
PatternGeneratorJRL::HandAbsolutePosition_t::theta
double theta
Definition: pgtypes.hh:177
PatternGeneratorJRL::SupportFeet_s::x
double x
Definition: pgtypes.hh:229
x
doublereal * x
Definition: qld.cpp:386
PatternGeneratorJRL::COMPosition_s::pitch
double pitch
Definition: pgtypes.hh:57
PatternGeneratorJRL::RelativeFootPosition_s::theta
double theta
Definition: pgtypes.hh:97
PatternGeneratorJRL::Circle_t::x_0
double x_0
Definition: pgtypes.hh:261
PatternGeneratorJRL::ZMPPosition_s::time
double time
Definition: pgtypes.hh:122
PatternGeneratorJRL::FootAbsolutePosition_t::ddomega2
double ddomega2
Definition: pgtypes.hh:145
PatternGeneratorJRL::HandAbsolutePosition_t::ddomega
double ddomega
Definition: pgtypes.hh:181
PatternGeneratorJRL::COMPosition_s::roll
double roll
Definition: pgtypes.hh:58
PatternGeneratorJRL::HandAbsolutePosition_t::dy
double dy
Definition: pgtypes.hh:179
PatternGeneratorJRL::LinearConstraintInequality_s::Center
Eigen::VectorXd Center
Definition: pgtypes.hh:212
PatternGeneratorJRL::HandAbsolutePosition_t::y
double y
Definition: pgtypes.hh:177
PatternGeneratorJRL::FootAbsolutePosition_t::dx
double dx
Definition: pgtypes.hh:143
WALK_GEN_JRL_EXPORT
#define WALK_GEN_JRL_EXPORT
Definition: pgtypes.hh:41
PatternGeneratorJRL::HandAbsolutePosition_t::dddomega2
double dddomega2
Definition: pgtypes.hh:183
PatternGeneratorJRL::FootAbsolutePosition_t::y
double y
Definition: pgtypes.hh:141
PatternGeneratorJRL::FootAbsolutePosition_t::omega2
double omega2
Definition: pgtypes.hh:141
PatternGeneratorJRL::operator<<
std::ostream & operator<<(std::ostream &os, const COMPosition_s &aCp)
Definition: pgtypes.hh:63
PatternGeneratorJRL::FootAbsolutePosition_t::dddy
double dddy
Definition: pgtypes.hh:147
PatternGeneratorJRL::FootAbsolutePosition_t::dddz
double dddz
Definition: pgtypes.hh:147
PatternGeneratorJRL::ControlLoopOneStepArgs::LeftFootPosition
FootAbsolutePosition LeftFootPosition
Definition: pgtypes.hh:279
PatternGeneratorJRL::Circle_t::margin
double margin
Definition: pgtypes.hh:264
PatternGeneratorJRL::FootAbsolutePosition_t::dy
double dy
Definition: pgtypes.hh:143
PatternGeneratorJRL::FootAbsolutePosition_t
Structure to store the absolute foot position.
Definition: pgtypes.hh:139
PatternGeneratorJRL::HandAbsolutePosition_t::omega2
double omega2
Definition: pgtypes.hh:177
PatternGeneratorJRL::ControlLoopOneStepArgs::CurrentVelocity
Eigen::VectorXd CurrentVelocity
Definition: pgtypes.hh:275
PatternGeneratorJRL::COMPosition_s::yaw
double yaw
Definition: pgtypes.hh:56
PatternGeneratorJRL::HandAbsolutePosition_t::ddz
double ddz
Definition: pgtypes.hh:181
PatternGeneratorJRL::SupportFeet_s
Definition: pgtypes.hh:228
PatternGeneratorJRL::Circle_t::r
double r
Definition: pgtypes.hh:263
PatternGeneratorJRL::FootAbsolutePosition_t::dddx
double dddx
Definition: pgtypes.hh:147
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t
Structure to store the absolute reference.
Definition: pgtypes.hh:241
PatternGeneratorJRL::LinearConstraintInequality_s
Definition: pgtypes.hh:209
PatternGeneratorJRL::FootAbsolutePosition_t::domega
double domega
Definition: pgtypes.hh:143
PatternGeneratorJRL::FootAbsolutePosition_t::dddomega
double dddomega
Definition: pgtypes.hh:147
PatternGeneratorJRL::COMPosition_s::z
double z[3]
Definition: pgtypes.hh:55
PatternGeneratorJRL::ZMPPosition_s::py
double py
Definition: pgtypes.hh:120
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::z
double z
Definition: pgtypes.hh:243
PatternGeneratorJRL::HandAbsolutePosition_t::dddomega
double dddomega
Definition: pgtypes.hh:183
PatternGeneratorJRL::Circle_t
Structure to model a circle (e.g : a stricly convex obstable)
Definition: pgtypes.hh:260
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::y
double y
Definition: pgtypes.hh:243
PatternGeneratorJRL::RelativeFootPosition_s::sz
double sz
Definition: pgtypes.hh:97
PatternGeneratorJRL::ControlLoopOneStepArgs::Momentum
Eigen::VectorXd Momentum
Definition: pgtypes.hh:281
PatternGeneratorJRL::COMPosition_s
Structure to store the COM position computed by the preview control.
Definition: pgtypes.hh:53
PatternGeneratorJRL::FootAbsolutePosition_t::dz
double dz
Definition: pgtypes.hh:143
PatternGeneratorJRL::HandAbsolutePosition_t::omega
double omega
Definition: pgtypes.hh:177
PatternGeneratorJRL::ZMPPosition_s::stepType
int stepType
Definition: pgtypes.hh:123
PatternGeneratorJRL::FootAbsolutePosition_t::theta
double theta
Definition: pgtypes.hh:141
PatternGeneratorJRL::HandAbsolutePosition_t::x
double x
Definition: pgtypes.hh:177
PatternGeneratorJRL::HandAbsolutePosition_t::ddtheta
double ddtheta
Definition: pgtypes.hh:181
PatternGeneratorJRL::SupportFeet_s::theta
double theta
Definition: pgtypes.hh:229
PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_s::StepNumber
int StepNumber
Definition: pgtypes.hh:222
PatternGeneratorJRL::RelativeFootPosition_s::sx
double sx
Definition: pgtypes.hh:97
PatternGeneratorJRL::HandAbsolutePosition_t
Structure to store the absolute foot position.
Definition: pgtypes.hh:175
PatternGeneratorJRL::ControlLoopOneStepArgs::RightFootPosition
FootAbsolutePosition RightFootPosition
Definition: pgtypes.hh:280
PatternGeneratorJRL::HandAbsolutePosition_t::dx
double dx
Definition: pgtypes.hh:179
PatternGeneratorJRL::RelativeFootPosition_s
Definition: pgtypes.hh:96
PatternGeneratorJRL::FootAbsolutePosition_t::ddy
double ddy
Definition: pgtypes.hh:145
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::x
double x
Definition: pgtypes.hh:243
PatternGeneratorJRL::FootAbsolutePosition_t::domega2
double domega2
Definition: pgtypes.hh:143
PatternGeneratorJRL::LinearConstraintInequality_s::EndingTime
double EndingTime
Definition: pgtypes.hh:214
PatternGeneratorJRL::FootAbsolutePosition_t::z
double z
Definition: pgtypes.hh:141
PatternGeneratorJRL::FootAbsolutePosition_t::dtheta
double dtheta
Definition: pgtypes.hh:143
PatternGeneratorJRL::ControlLoopOneStepArgs
Definition: pgtypes.hh:273
PatternGeneratorJRL::FootAbsolutePosition_t::ddomega
double ddomega
Definition: pgtypes.hh:145
PatternGeneratorJRL::RelativeFootPosition_s::DStime
double DStime
Definition: pgtypes.hh:99
PatternGeneratorJRL::HandAbsolutePosition_t::ddx
double ddx
Definition: pgtypes.hh:181
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::RefVectorTheta
Eigen::VectorXd RefVectorTheta
Definition: pgtypes.hh:248
PatternGeneratorJRL::HandAbsolutePosition_t::ddomega2
double ddomega2
Definition: pgtypes.hh:181
PatternGeneratorJRL::HandAbsolutePosition_t::domega
double domega
Definition: pgtypes.hh:179
PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_s::Dc
Eigen::MatrixXd Dc
Definition: pgtypes.hh:221
PatternGeneratorJRL
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PatternGeneratorJRL::HandAbsolutePosition_t::dddy
double dddy
Definition: pgtypes.hh:183
PatternGeneratorJRL::RelativeFootPosition_s::DeviationHipHeight
double DeviationHipHeight
Definition: pgtypes.hh:103
PatternGeneratorJRL::RelativeFootPosition_s::sy
double sy
Definition: pgtypes.hh:97
PatternGeneratorJRL::LinearConstraintInequality_s::A
Eigen::MatrixXd A
Definition: pgtypes.hh:210
PatternGeneratorJRL::COMPosition_s::y
double y[3]
Definition: pgtypes.hh:54
PatternGeneratorJRL::COMState_s
Structure to store the COM state computed by the preview control.
Definition: pgtypes.hh:76
PatternGeneratorJRL::ZMPPosition_s::pz
double pz
Definition: pgtypes.hh:120
PatternGeneratorJRL::HandAbsolutePosition_t::dddz
double dddz
Definition: pgtypes.hh:183
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::dYaw
double dYaw
Definition: pgtypes.hh:243
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::RefVectorX
Eigen::VectorXd RefVectorX
Definition: pgtypes.hh:246
PatternGeneratorJRL::FootAbsolutePosition_t::dddtheta
double dddtheta
Definition: pgtypes.hh:147
PatternGeneratorJRL::FootAbsolutePosition_t::ddtheta
double ddtheta
Definition: pgtypes.hh:145
PatternGeneratorJRL::ControlLoopOneStepArgs::finalCOMState
COMState finalCOMState
Definition: pgtypes.hh:278
PatternGeneratorJRL::ControlLoopOneStepArgs::CurrentConfiguration
Eigen::VectorXd CurrentConfiguration
Definition: pgtypes.hh:274
PatternGeneratorJRL::SupportFeet_s::SupportFoot
int SupportFoot
Definition: pgtypes.hh:230
PatternGeneratorJRL::ReferenceAbsoluteVelocity_t::RefVectorY
Eigen::VectorXd RefVectorY
Definition: pgtypes.hh:247
PatternGeneratorJRL::ControlLoopOneStepArgs::CurrentAcceleration
Eigen::VectorXd CurrentAcceleration
Definition: pgtypes.hh:276
PatternGeneratorJRL::ControlLoopOneStepArgs::ZMPTarget
Eigen::VectorXd ZMPTarget
Definition: pgtypes.hh:277
PatternGeneratorJRL::Circle_t::y_0
double y_0
Definition: pgtypes.hh:262
PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_s::D
Eigen::MatrixXd D
Definition: pgtypes.hh:220
PatternGeneratorJRL::LinearConstraintInequality_s::StartingTime
double StartingTime
Definition: pgtypes.hh:214
PatternGeneratorJRL::HandAbsolutePosition_t::stepType
int stepType
Definition: pgtypes.hh:189
PatternGeneratorJRL::HandAbsolutePosition_t::time
double time
Definition: pgtypes.hh:185
PatternGeneratorJRL::LinearConstraintInequalityFreeFeet_s
Linear constraints with variable feet placement.
Definition: pgtypes.hh:219
PatternGeneratorJRL::COMPosition_s::x
double x[3]
Definition: pgtypes.hh:54