next-step.h
Go to the documentation of this file.
1 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2  * Copyright Projet JRL-Japan, 2007
3  *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4  *
5  * File: NextStep.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 #ifndef __SOT_SOTNEXTSTEP_H__
21 #define __SOT_SOTNEXTSTEP_H__
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <pinocchio/fwd.hpp>
28 
29 /* SOT */
30 #include <dynamic-graph/entity.h>
31 #include <dynamic-graph/signal-ptr.h>
32 #include <dynamic-graph/signal-time-dependent.h>
33 
34 #include <sot/core/matrix-geometry.hh>
35 
36 /* STD */
37 #include <deque>
38 #include <string>
39 
40 /* --------------------------------------------------------------------- */
41 /* --- API ------------------------------------------------------------- */
42 /* --------------------------------------------------------------------- */
43 
44 #if defined(WIN32)
45 #if defined(next_step_EXPORTS)
46 #define SOTNEXTSTEP_EXPORT __declspec(dllexport)
47 #else
48 #define SOTNEXTSTEP_EXPORT __declspec(dllimport)
49 #endif
50 #else
51 #define SOTNEXTSTEP_EXPORT
52 #endif
53 
54 namespace dynamicgraph {
55 namespace sot {
56 /* --------------------------------------------------------------------- */
57 /* --- CLASS ----------------------------------------------------------- */
58 /* --------------------------------------------------------------------- */
59 
60 /* --- Two Hand Observer ----------------------------------------------- */
61 
69  public:
70  SignalPtr<MatrixHomogeneous, int> referencePositionLeftSIN;
71  SignalPtr<Vector, int> referenceVelocityLeftSIN;
72  SignalPtr<Vector, int> referenceAccelerationLeftSIN;
73  SignalPtr<MatrixHomogeneous, int> leftFootPositionSIN;
74 
75  SignalPtr<MatrixHomogeneous, int> referencePositionRightSIN;
76  SignalPtr<Vector, int> referenceVelocityRightSIN;
77  SignalPtr<Vector, int> referenceAccelerationRightSIN;
78  SignalPtr<MatrixHomogeneous, int> rightFootPositionSIN;
79 
80  SignalTimeDependent<MatrixHomogeneous, int> referencePositionLeftSOUT;
81  SignalTimeDependent<MatrixHomogeneous, int> referencePositionRightSOUT;
82  SignalTimeDependent<Vector, int> referenceVelocitySOUT;
83  SignalTimeDependent<Vector, int> referenceAccelerationSOUT;
84 
85  public:
86  NextStepTwoHandObserver(const std::string &name);
87 
88  MatrixHomogeneous &computeReferencePositionLeft(MatrixHomogeneous &res,
89  int timeCurr);
90  MatrixHomogeneous &computeReferencePositionRight(MatrixHomogeneous &res,
91  int timeCurr);
92  Vector &computeReferenceVelocity(const Vector &right, const Vector &left,
93  Vector &res);
94  Vector &computeReferenceAcceleration(const Vector &right, const Vector &left,
95  Vector &res);
96 
97  SignalArray<int> getSignals(void);
98  operator SignalArray<int>();
99 
100  private:
101  MatrixHomogeneous &computeRefPos(MatrixHomogeneous &res, int timeCurr,
102  const MatrixHomogeneous &wMsf);
103 };
104 
105 /* --- Next Step (stepper) --------------------------------------------- */
106 
115 class SOTNEXTSTEP_EXPORT NextStep : public Entity {
116  public:
118 
119  protected: /* --- FOOT PRINT LIST --- */
121  class FootPrint {
122  public:
123  double x, y, theta;
126  };
127  std::deque<FootPrint> footPrintList;
128 
129  protected: /* --- INTRODUCTION PERIOD --- */
130  unsigned int period;
131  static const unsigned int PERIOD_DEFAULT;
133 
134  protected: /* --- STATE --- */
135  enum SteppingMode { MODE_1D, MODE_3D };
138  STATE_STARTING
139  ,
140  STATE_STOPING
141  // and stop.
142  ,
143  STATE_STARTED
144  ,
145  STATE_STOPED
146  };
148 
149  protected: /* --- STEPPING --- */
151  static const double ZERO_STEP_POSITION_DEFAULT; // = .19
152 
153  protected: /* --- REFERENCE FRAME --- */
154  MatrixHomogeneous rfMref0;
155  MatrixHomogeneous lfMref0;
157 
158  void thisIsZero();
159 
160  protected: /* --- DEBUG --- */
161  std::ostream *verbose;
162 
163  public: /* --- CONSTRUCTION --- */
164  NextStep(const std::string &name);
165  virtual ~NextStep(void);
166 
167  public: /* --- Signal --- */
168  SignalPtr<MatrixHomogeneous, int> referencePositionLeftSIN;
169  SignalPtr<MatrixHomogeneous, int> referencePositionRightSIN;
170  SignalPtr<unsigned, int> contactFootSIN;
171 
172  Signal<int, int> triggerSOUT;
173 
174  public: /* --- FUNCTIONS --- */
175  virtual void nextStep(const int &timeCurr);
176  virtual void starter(const int &timeCurr);
177  virtual void stoper(const int &timeCurr);
178 
179  virtual void introductionCallBack(const int &){};
180 
181  int &triggerCall(int &dummy, int timeCurr);
182 
183  public: /* --- PARAMS --- */
184  virtual void display(std::ostream &os) const;
185  virtual void commandLine(const std::string &cmdLine,
186  std::istringstream &cmdArgs, std::ostream &os);
187 };
188 
189 } // namespace sot
190 } // namespace dynamicgraph
191 
192 #endif // #ifndef __SOT_SOTNEXTSTEP_H__
NextStepTwoHandObserver(const std::string &name)
Vector & computeReferenceAcceleration(const Vector &right, const Vector &left, Vector &res)
SignalTimeDependent< MatrixHomogeneous, int > referencePositionLeftSOUT
Definition: next-step.h:80
Vector & computeReferenceVelocity(const Vector &right, const Vector &left, Vector &res)
SignalTimeDependent< Vector, int > referenceAccelerationSOUT
Definition: next-step.h:83
SignalPtr< Vector, int > referenceAccelerationRightSIN
Definition: next-step.h:77
SignalPtr< MatrixHomogeneous, int > leftFootPositionSIN
Definition: next-step.h:73
SignalPtr< MatrixHomogeneous, int > rightFootPositionSIN
Definition: next-step.h:78
SignalPtr< Vector, int > referenceVelocityLeftSIN
Definition: next-step.h:71
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:75
MatrixHomogeneous & computeReferencePositionRight(MatrixHomogeneous &res, int timeCurr)
MatrixHomogeneous & computeReferencePositionLeft(MatrixHomogeneous &res, int timeCurr)
SignalPtr< Vector, int > referenceAccelerationLeftSIN
Definition: next-step.h:72
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:70
SignalTimeDependent< MatrixHomogeneous, int > referencePositionRightSOUT
Definition: next-step.h:81
SignalTimeDependent< Vector, int > referenceVelocitySOUT
Definition: next-step.h:82
SignalPtr< Vector, int > referenceVelocityRightSIN
Definition: next-step.h:76
Definition: next-step.h:121
double theta
Definition: next-step.h:123
ContactName contact
Definition: next-step.h:124
int introductionTime
Definition: next-step.h:125
Definition: next-step.h:115
virtual void introductionCallBack(const int &)
Definition: next-step.h:179
int timeLastIntroduction
Definition: next-step.h:132
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:169
SteppingMode
Definition: next-step.h:135
@ MODE_1D
Definition: next-step.h:135
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:168
std::deque< FootPrint > footPrintList
Definition: next-step.h:127
virtual void starter(const int &timeCurr)
SteppingState
Definition: next-step.h:137
static const double ZERO_STEP_POSITION_DEFAULT
Definition: next-step.h:151
virtual void nextStep(const int &timeCurr)
SignalPtr< unsigned, int > contactFootSIN
Definition: next-step.h:170
int & triggerCall(int &dummy, int timeCurr)
ContactName
Definition: next-step.h:120
@ CONTACT_LEFT_FOOT
Definition: next-step.h:120
virtual void commandLine(const std::string &cmdLine, std::istringstream &cmdArgs, std::ostream &os)
SteppingState state
Definition: next-step.h:147
static const unsigned int PERIOD_DEFAULT
Definition: next-step.h:131
NextStep(const std::string &name)
SteppingMode mode
Definition: next-step.h:136
MatrixHomogeneous rfMref0
Definition: next-step.h:154
virtual void display(std::ostream &os) const
Signal< int, int > triggerSOUT
Definition: next-step.h:172
std::ostream * verbose
Definition: next-step.h:161
NextStepTwoHandObserver twoHandObserver
Definition: next-step.h:156
virtual void stoper(const int &timeCurr)
double zeroStepPosition
Definition: next-step.h:150
MatrixHomogeneous lfMref0
Definition: next-step.h:155
unsigned int period
Definition: next-step.h:130
@ CONTACT_RIGHT_FOOT
Definition: step-queue.h:58
Definition: exception-pg.h:47
#define SOTNEXTSTEP_EXPORT
Definition: next-step.h:51