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 <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
20namespace dynamicgraph {
21namespace sot {
22namespace tools {
23using dynamicgraph::Entity;
24using dynamicgraph::Signal;
25using dynamicgraph::Vector;
26using dynamicgraph::sot::MatrixHomogeneous;
27
28class 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
Definition: seqplay.hh:28
Definition: cubic-interpolation-se3.hh:16