1 |
|
|
// |
2 |
|
|
// Copyright (C) 2012, 2013 LAAS-CNRS |
3 |
|
|
// |
4 |
|
|
// Author: Florent Lamiraux, Mehdi Benallegue |
5 |
|
|
// |
6 |
|
|
|
7 |
|
|
#ifndef SOT_TOOLS_SEQPLAY_HH |
8 |
|
|
#define SOT_TOOLS_SEQPLAY_HH |
9 |
|
|
|
10 |
|
|
#include <dynamic-graph/entity.h> |
11 |
|
|
#include <dynamic-graph/factory.h> |
12 |
|
|
#include <dynamic-graph/linear-algebra.h> |
13 |
|
|
#include <dynamic-graph/signal.h> |
14 |
|
|
|
15 |
|
|
#include <fstream> |
16 |
|
|
#include <iostream> |
17 |
|
|
#include <sot/core/matrix-geometry.hh> |
18 |
|
|
#include <sstream> |
19 |
|
|
|
20 |
|
|
namespace dynamicgraph { |
21 |
|
|
namespace sot { |
22 |
|
|
namespace tools { |
23 |
|
|
using dynamicgraph::Entity; |
24 |
|
|
using dynamicgraph::Signal; |
25 |
|
|
using dynamicgraph::Vector; |
26 |
|
|
using dynamicgraph::sot::MatrixHomogeneous; |
27 |
|
|
|
28 |
|
|
class Seqplay : public Entity { |
29 |
|
|
Signal<Vector, int> postureSOUT_; |
30 |
|
|
Signal<MatrixHomogeneous, int> leftAnkleSOUT_; |
31 |
|
|
Signal<MatrixHomogeneous, int> rightAnkleSOUT_; |
32 |
|
|
Signal<Vector, int> leftAnkleVelSOUT_; |
33 |
|
|
Signal<Vector, int> rightAnkleVelSOUT_; |
34 |
|
|
Signal<Vector, int> comSOUT_; |
35 |
|
|
Signal<Vector, int> comdotSOUT_; |
36 |
|
|
Signal<Vector, int> comddotSOUT_; |
37 |
|
|
Signal<Vector, int> forceLeftFootSOUT_; |
38 |
|
|
Signal<Vector, int> forceRightFootSOUT_; |
39 |
|
|
|
40 |
|
|
Signal<Vector, int> zmpSOUT_; |
41 |
|
|
|
42 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
43 |
|
|
Seqplay(const std::string& name); |
44 |
|
|
|
45 |
|
|
void load(const std::string& filename); |
46 |
|
|
void start(); |
47 |
|
|
virtual std::string getDocString() const; |
48 |
|
|
|
49 |
|
|
private: |
50 |
|
|
Vector& computePosture(Vector& pos, const int& t); |
51 |
|
|
MatrixHomogeneous& computeLeftAnkle(MatrixHomogeneous& la, const int& t); |
52 |
|
|
MatrixHomogeneous& computeRightAnkle(MatrixHomogeneous& ra, const int& t); |
53 |
|
|
Vector& computeAnkleVelocity( |
54 |
|
|
Vector& velocity, const std::vector<MatrixHomogeneous>& ankleVector, |
55 |
|
|
const int& t); |
56 |
|
|
Vector& computeLeftAnkleVel(Vector& velocity, const int& t); |
57 |
|
|
Vector& computeRightAnkleVel(Vector& velocity, const int& t); |
58 |
|
|
Vector& computeCom(Vector& com, const int& t); |
59 |
|
|
Vector& computeComdot(Vector& comdot, const int& t); |
60 |
|
|
Vector& computeComddot(Vector& comdot, const int& t); |
61 |
|
|
Vector& computeZMP(Vector& comdot, const int& t); |
62 |
|
|
Vector& computeForceFoot(Vector&, const std::vector<Vector>&, const int&); |
63 |
|
|
Vector& computeForceLeftFoot(Vector& force, const int& t); |
64 |
|
|
Vector& computeForceRightFoot(Vector& force, const int& t); |
65 |
|
|
|
66 |
|
|
void readAnkleFile(std::ifstream&, std::vector<MatrixHomogeneous>&, |
67 |
|
|
const std::string&); |
68 |
|
|
void readForceFile(std::ifstream&, std::vector<Vector>&, const std::string&); |
69 |
|
|
// 0: motion not started, 1: motion in progress, 2: motion finished |
70 |
|
|
unsigned int state_; |
71 |
|
|
unsigned int configId_; |
72 |
|
|
int startTime_; |
73 |
|
|
|
74 |
|
|
std::vector<Vector> posture_; |
75 |
|
|
std::vector<MatrixHomogeneous> leftAnkle_; |
76 |
|
|
std::vector<MatrixHomogeneous> rightAnkle_; |
77 |
|
|
|
78 |
|
|
std::vector<Vector> leftAnkleDot_; |
79 |
|
|
std::vector<Vector> rightAnkleDot_; |
80 |
|
|
|
81 |
|
|
std::vector<Vector> com_; |
82 |
|
|
std::vector<Vector> comdot_; |
83 |
|
|
std::vector<Vector> comddot_; |
84 |
|
|
std::vector<Vector> zmp_; |
85 |
|
|
|
86 |
|
|
bool facultativeFound_[7]; |
87 |
|
|
|
88 |
|
|
std::vector<Vector> forceLeftFoot_; |
89 |
|
|
std::vector<Vector> forceRightFoot_; |
90 |
|
|
std::vector<double> time_; |
91 |
|
|
|
92 |
|
|
// Temporary variables for internal computations |
93 |
|
|
MatrixRotation R0_, R0t_, R1_, R1R0t_; |
94 |
|
|
}; // class Seqplay |
95 |
|
|
} // namespace tools |
96 |
|
|
} // namespace sot |
97 |
|
|
} // namespace dynamicgraph |
98 |
|
|
|
99 |
|
|
#endif // SOT_TOOLS_SEQPLAY_HH |