7 #ifndef SOT_TOOLS_SEQPLAY_HH 8 #define SOT_TOOLS_SEQPLAY_HH 14 #include <dynamic-graph/entity.h> 15 #include <dynamic-graph/factory.h> 16 #include <dynamic-graph/linear-algebra.h> 17 #include <dynamic-graph/signal.h> 18 #include <sot/core/matrix-geometry.hh> 23 using dynamicgraph::Entity;
24 using dynamicgraph::Signal;
25 using dynamicgraph::Vector;
26 using dynamicgraph::sot::MatrixHomogeneous;
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_;
40 Signal<Vector, int> zmpSOUT_;
42 DYNAMIC_GRAPH_ENTITY_DECL();
43 Seqplay(
const std::string& name);
45 void load(
const std::string& filename);
47 virtual std::string getDocString()
const;
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(Vector& velocity,
const std::vector<MatrixHomogeneous>& ankleVector,
const int& t);
54 Vector& computeLeftAnkleVel(Vector& velocity,
const int& t);
55 Vector& computeRightAnkleVel(Vector& velocity,
const int& t);
56 Vector& computeCom(Vector& com,
const int& t);
57 Vector& computeComdot(Vector& comdot,
const int& t);
58 Vector& computeComddot(Vector& comdot,
const int& t);
59 Vector& computeZMP(Vector& comdot,
const int& t);
60 Vector& computeForceFoot(Vector&,
const std::vector<Vector>&,
const int&);
61 Vector& computeForceLeftFoot(Vector& force,
const int& t);
62 Vector& computeForceRightFoot(Vector& force,
const int& t);
64 void readAnkleFile(std::ifstream&, std::vector<MatrixHomogeneous>&,
const std::string&);
65 void readForceFile(std::ifstream&, std::vector<Vector>&,
const std::string&);
68 unsigned int configId_;
71 std::vector<Vector> posture_;
72 std::vector<MatrixHomogeneous> leftAnkle_;
73 std::vector<MatrixHomogeneous> rightAnkle_;
75 std::vector<Vector> leftAnkleDot_;
76 std::vector<Vector> rightAnkleDot_;
78 std::vector<Vector> com_;
79 std::vector<Vector> comdot_;
80 std::vector<Vector> comddot_;
81 std::vector<Vector> zmp_;
83 bool facultativeFound_[7];
85 std::vector<Vector> forceLeftFoot_;
86 std::vector<Vector> forceRightFoot_;
87 std::vector<double> time_;
90 MatrixRotation R0_, R0t_, R1_, R1R0t_;
96 #endif // SOT_TOOLS_SEQPLAY_HH
Definition: cubic-interpolation-se3.hh:15