Loading...
Searching...
No Matches
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
54namespace dynamicgraph {
55namespace 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
115class 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
140 STATE_STOPING
141 // and stop.
143 STATE_STARTED
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
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)
SignalTimeDependent< MatrixHomogeneous, int > referencePositionLeftSOUT
Definition next-step.h:80
Vector & computeReferenceVelocity(const Vector &right, const Vector &left, Vector &res)
MatrixHomogeneous & computeReferencePositionRight(MatrixHomogeneous &res, int timeCurr)
SignalTimeDependent< Vector, int > referenceAccelerationSOUT
Definition next-step.h:83
SignalPtr< Vector, int > referenceAccelerationRightSIN
Definition next-step.h:77
MatrixHomogeneous & computeReferencePositionLeft(MatrixHomogeneous &res, int timeCurr)
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
Vector & computeReferenceAcceleration(const Vector &right, const Vector &left, Vector &res)
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
int & triggerCall(int &dummy, int timeCurr)
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
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