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