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:
117  DYNAMIC_GRAPH_ENTITY_DECL();
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__
dynamicgraph::sot::NextStepTwoHandObserver::referencePositionRightSOUT
SignalTimeDependent< MatrixHomogeneous, int > referencePositionRightSOUT
Definition: next-step.h:81
dynamicgraph::sot::NextStep::FootPrint::introductionTime
int introductionTime
Definition: next-step.h:125
dynamicgraph::sot::NextStep::lfMref0
MatrixHomogeneous lfMref0
Definition: next-step.h:155
dynamicgraph::sot::NextStep::SteppingMode
SteppingMode
Definition: next-step.h:135
dynamicgraph::sot::NextStepTwoHandObserver::referencePositionRightSIN
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:75
dynamicgraph::sot::NextStep::introductionCallBack
virtual void introductionCallBack(const int &)
Definition: next-step.h:179
dynamicgraph::sot::NextStep::referencePositionLeftSIN
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:168
dynamicgraph::sot::NextStepTwoHandObserver::referenceAccelerationSOUT
SignalTimeDependent< Vector, int > referenceAccelerationSOUT
Definition: next-step.h:83
dynamicgraph::sot::NextStepTwoHandObserver::referenceVelocityRightSIN
SignalPtr< Vector, int > referenceVelocityRightSIN
Definition: next-step.h:76
dynamicgraph::sot::NextStepTwoHandObserver::referenceVelocitySOUT
SignalTimeDependent< Vector, int > referenceVelocitySOUT
Definition: next-step.h:82
dynamicgraph::sot::CONTACT_LEFT_FOOT
@ CONTACT_LEFT_FOOT
Definition: step-queue.h:58
SOTNEXTSTEP_EXPORT
#define SOTNEXTSTEP_EXPORT
Definition: next-step.h:51
dynamicgraph::sot::NextStep::referencePositionRightSIN
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:169
dynamicgraph
Definition: exception-pg.h:47
dynamicgraph::sot::NextStep::rfMref0
MatrixHomogeneous rfMref0
Definition: next-step.h:154
dynamicgraph::sot::NextStepTwoHandObserver::referenceAccelerationRightSIN
SignalPtr< Vector, int > referenceAccelerationRightSIN
Definition: next-step.h:77
dynamicgraph::sot::NextStepTwoHandObserver::referenceAccelerationLeftSIN
SignalPtr< Vector, int > referenceAccelerationLeftSIN
Definition: next-step.h:72
dynamicgraph::sot::NextStep::timeLastIntroduction
int timeLastIntroduction
Definition: next-step.h:132
dynamicgraph::sot::NextStepTwoHandObserver::leftFootPositionSIN
SignalPtr< MatrixHomogeneous, int > leftFootPositionSIN
Definition: next-step.h:73
dynamicgraph::sot::NextStepTwoHandObserver::referenceVelocityLeftSIN
SignalPtr< Vector, int > referenceVelocityLeftSIN
Definition: next-step.h:71
dynamicgraph::sot::NextStep::state
SteppingState state
Definition: next-step.h:147
dynamicgraph::sot::NextStepTwoHandObserver
Definition: next-step.h:68
dynamicgraph::sot::NextStep::period
unsigned int period
Definition: next-step.h:130
dynamicgraph::sot::NextStep::SteppingState
SteppingState
Definition: next-step.h:137
dynamicgraph::sot::NextStepTwoHandObserver::rightFootPositionSIN
SignalPtr< MatrixHomogeneous, int > rightFootPositionSIN
Definition: next-step.h:78
dynamicgraph::sot::NextStep::FootPrint
Definition: next-step.h:121
dynamicgraph::sot::NextStep::zeroStepPosition
double zeroStepPosition
Definition: next-step.h:150
dynamicgraph::sot::NextStep::ZERO_STEP_POSITION_DEFAULT
static const double ZERO_STEP_POSITION_DEFAULT
Definition: next-step.h:151
dynamicgraph::sot::NextStep
Definition: next-step.h:115
dynamicgraph::sot::NextStepTwoHandObserver::referencePositionLeftSOUT
SignalTimeDependent< MatrixHomogeneous, int > referencePositionLeftSOUT
Definition: next-step.h:80
dynamicgraph::sot::NextStep::footPrintList
std::deque< FootPrint > footPrintList
Definition: next-step.h:127
dynamicgraph::sot::NextStep::mode
SteppingMode mode
Definition: next-step.h:136
dynamicgraph::sot::NextStep::FootPrint::contact
ContactName contact
Definition: next-step.h:124
dynamicgraph::sot::NextStep::contactFootSIN
SignalPtr< unsigned, int > contactFootSIN
Definition: next-step.h:170
dynamicgraph::sot::NextStepTwoHandObserver::referencePositionLeftSIN
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:70
dynamicgraph::sot::NextStep::twoHandObserver
NextStepTwoHandObserver twoHandObserver
Definition: next-step.h:156
dynamicgraph::sot::NextStep::PERIOD_DEFAULT
static const unsigned int PERIOD_DEFAULT
Definition: next-step.h:131
dynamicgraph::sot::NextStep::triggerSOUT
Signal< int, int > triggerSOUT
Definition: next-step.h:172
dynamicgraph::sot::NextStep::FootPrint::y
double y
Definition: next-step.h:123
dynamicgraph::sot::CONTACT_RIGHT_FOOT
@ CONTACT_RIGHT_FOOT
Definition: step-queue.h:58
dynamicgraph::sot::NextStep::verbose
std::ostream * verbose
Definition: next-step.h:161
dynamicgraph::sot::NextStep::ContactName
ContactName
Definition: next-step.h:120