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#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
53namespace dynamicgraph {
54namespace 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
114class SOTNEXTSTEP_EXPORT NextStep : public Entity {
115 public:
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
139 STATE_STOPING
140 // and stop.
142 STATE_STARTED
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
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__
NextStepTwoHandObserver(const std::string &name)
SignalTimeDependent< MatrixHomogeneous, int > referencePositionLeftSOUT
Definition: next-step.h:79
Vector & computeReferenceVelocity(const Vector &right, const Vector &left, Vector &res)
MatrixHomogeneous & computeReferencePositionRight(MatrixHomogeneous &res, int timeCurr)
SignalTimeDependent< Vector, int > referenceAccelerationSOUT
Definition: next-step.h:82
SignalPtr< Vector, int > referenceAccelerationRightSIN
Definition: next-step.h:76
MatrixHomogeneous & computeReferencePositionLeft(MatrixHomogeneous &res, int timeCurr)
SignalPtr< MatrixHomogeneous, int > leftFootPositionSIN
Definition: next-step.h:72
SignalPtr< MatrixHomogeneous, int > rightFootPositionSIN
Definition: next-step.h:77
SignalPtr< Vector, int > referenceVelocityLeftSIN
Definition: next-step.h:70
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:74
Vector & computeReferenceAcceleration(const Vector &right, const Vector &left, Vector &res)
SignalPtr< Vector, int > referenceAccelerationLeftSIN
Definition: next-step.h:71
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:69
SignalTimeDependent< MatrixHomogeneous, int > referencePositionRightSOUT
Definition: next-step.h:80
SignalTimeDependent< Vector, int > referenceVelocitySOUT
Definition: next-step.h:81
SignalPtr< Vector, int > referenceVelocityRightSIN
Definition: next-step.h:75
Definition: next-step.h:120
double theta
Definition: next-step.h:122
ContactName contact
Definition: next-step.h:123
int introductionTime
Definition: next-step.h:124
Definition: next-step.h:114
virtual void introductionCallBack(const int &)
Definition: next-step.h:178
int timeLastIntroduction
Definition: next-step.h:131
SignalPtr< MatrixHomogeneous, int > referencePositionRightSIN
Definition: next-step.h:168
SteppingMode
Definition: next-step.h:134
@ MODE_1D
Definition: next-step.h:134
SignalPtr< MatrixHomogeneous, int > referencePositionLeftSIN
Definition: next-step.h:167
std::deque< FootPrint > footPrintList
Definition: next-step.h:126
int & triggerCall(int &dummy, int timeCurr)
virtual void starter(const int &timeCurr)
SteppingState
Definition: next-step.h:136
static const double ZERO_STEP_POSITION_DEFAULT
Definition: next-step.h:150
virtual void nextStep(const int &timeCurr)
SignalPtr< unsigned, int > contactFootSIN
Definition: next-step.h:169
ContactName
Definition: next-step.h:119
@ CONTACT_LEFT_FOOT
Definition: next-step.h:119
virtual void commandLine(const std::string &cmdLine, std::istringstream &cmdArgs, std::ostream &os)
SteppingState state
Definition: next-step.h:146
static const unsigned int PERIOD_DEFAULT
Definition: next-step.h:130
NextStep(const std::string &name)
SteppingMode mode
Definition: next-step.h:135
MatrixHomogeneous rfMref0
Definition: next-step.h:153
virtual void display(std::ostream &os) const
Signal< int, int > triggerSOUT
Definition: next-step.h:171
std::ostream * verbose
Definition: next-step.h:160
NextStepTwoHandObserver twoHandObserver
Definition: next-step.h:155
virtual void stoper(const int &timeCurr)
double zeroStepPosition
Definition: next-step.h:149
MatrixHomogeneous lfMref0
Definition: next-step.h:154
unsigned int period
Definition: next-step.h:129
@ CONTACT_RIGHT_FOOT
Definition: step-queue.h:58
Definition: exception-pg.h:47
#define SOTNEXTSTEP_EXPORT
Definition: next-step.h:50