seqplay.hh
Go to the documentation of this file.
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 <iostream>
11 #include <sstream>
12 #include <fstream>
13 
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>
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(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);
63 
64  void readAnkleFile(std::ifstream&, std::vector<MatrixHomogeneous>&, const std::string&);
65  void readForceFile(std::ifstream&, std::vector<Vector>&, const std::string&);
66  // 0: motion not started, 1: motion in progress, 2: motion finished
67  unsigned int state_;
68  unsigned int configId_;
69  int startTime_;
70 
71  std::vector<Vector> posture_;
72  std::vector<MatrixHomogeneous> leftAnkle_;
73  std::vector<MatrixHomogeneous> rightAnkle_;
74 
75  std::vector<Vector> leftAnkleDot_;
76  std::vector<Vector> rightAnkleDot_;
77 
78  std::vector<Vector> com_;
79  std::vector<Vector> comdot_;
80  std::vector<Vector> comddot_;
81  std::vector<Vector> zmp_;
82 
83  bool facultativeFound_[7];
84 
85  std::vector<Vector> forceLeftFoot_;
86  std::vector<Vector> forceRightFoot_;
87  std::vector<double> time_;
88 
89  // Temporary variables for internal computations
90  MatrixRotation R0_, R0t_, R1_, R1R0t_;
91 }; // class Seqplay
92 } // namespace tools
93 } // namespace sot
94 } // namespace dynamicgraph
95 
96 #endif // SOT_TOOLS_SEQPLAY_HH
Definition: seqplay.hh:28
Definition: cubic-interpolation-se3.hh:15