1 |
|
|
// |
2 |
|
|
// Copyright (C) 2012, 2013 LAAS-CNRS |
3 |
|
|
// |
4 |
|
|
// From Author: Florent Lamiraux, Mehdi Benallegue, |
5 |
|
|
// Author: Olivier Stasse |
6 |
|
|
// Simple sequence player just playing back a set of poses. |
7 |
|
|
// |
8 |
|
|
|
9 |
|
|
#ifndef SOT_TOOLS_SIMPLE_SEQPLAY_HH |
10 |
|
|
#define SOT_TOOLS_SIMPLE_SEQPLAY_HH |
11 |
|
|
|
12 |
|
|
#include <dynamic-graph/entity.h> |
13 |
|
|
#include <dynamic-graph/factory.h> |
14 |
|
|
#include <dynamic-graph/linear-algebra.h> |
15 |
|
|
#include <dynamic-graph/signal-ptr.h> |
16 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
17 |
|
|
|
18 |
|
|
#include <fstream> |
19 |
|
|
#include <iostream> |
20 |
|
|
#include <sot/core/matrix-geometry.hh> |
21 |
|
|
#include <sstream> |
22 |
|
|
|
23 |
|
|
namespace dynamicgraph { |
24 |
|
|
namespace sot { |
25 |
|
|
namespace tools { |
26 |
|
|
namespace dg = dynamicgraph; |
27 |
|
|
|
28 |
|
|
class SimpleSeqPlay : public dg::Entity { |
29 |
|
|
public: |
30 |
|
|
typedef int Dummy; |
31 |
|
|
dg::SignalTimeDependent<Dummy, int> firstSINTERN; |
32 |
|
|
dg::SignalTimeDependent<dg::Vector, int> postureSOUT_; |
33 |
|
|
|
34 |
|
|
dg::SignalPtr<dg::Vector, int> currentPostureSIN_; |
35 |
|
|
|
36 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
37 |
|
|
SimpleSeqPlay(const std::string& name); |
38 |
|
|
|
39 |
|
|
void load(const std::string& filename); |
40 |
|
|
void start(); |
41 |
|
|
virtual std::string getDocString() const; |
42 |
|
|
|
43 |
|
|
bool waiting() const; |
44 |
|
|
bool initializing() const; |
45 |
|
|
bool executing() const; |
46 |
|
|
bool finished() const; |
47 |
|
|
|
48 |
|
|
private: |
49 |
|
|
dg::Vector& computePosture(dg::Vector& pos, int t); |
50 |
|
|
// 0: motion not started, |
51 |
|
|
// 1: going to the current position to the first position. |
52 |
|
|
// 2: motion in progress, 3: motion finished |
53 |
|
|
unsigned int state_; |
54 |
|
|
int startTime_; |
55 |
|
|
|
56 |
|
|
std::vector<dg::Vector> posture_; |
57 |
|
|
dg::Vector currentPosture_; |
58 |
|
|
|
59 |
|
|
std::vector<double> time_; |
60 |
|
|
double dt_; |
61 |
|
|
/// Time to start. |
62 |
|
|
double time_to_start_; |
63 |
|
|
// Number of iterations performed in state1. |
64 |
|
|
int it_nbs_in_state1_; |
65 |
|
|
|
66 |
|
|
}; // class SimpleSeqPlay |
67 |
|
|
} // namespace tools |
68 |
|
|
} // namespace sot |
69 |
|
|
} // namespace dynamicgraph |
70 |
|
|
|
71 |
|
|
#endif // SOT_TOOLS_SIMPLESEQPLAY_HH |