UpperBodyMotion.cpp 4.81 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
 *
 * Francois Keith
 * Olivier Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
22
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
23
24
25
26
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */

/*! This object generate all the values for the upperbody motion */
27
#include <MotionGeneration/UpperBodyMotion.hh>
28
29
#include <fstream>
#include <iostream>
Thomas Moulard's avatar
Thomas Moulard committed
30

31
using namespace ::PatternGeneratorJRL;
Thomas Moulard's avatar
Thomas Moulard committed
32

33
UpperBodyMotion::UpperBodyMotion() {}
Thomas Moulard's avatar
Thomas Moulard committed
34

35
UpperBodyMotion::~UpperBodyMotion() {}
Thomas Moulard's avatar
Thomas Moulard committed
36

37
void UpperBodyMotion::GenerateDataFile(string aFileName, int LenghtDataArray) {
Thomas Moulard's avatar
Thomas Moulard committed
38
39
  ofstream aof;

40
41
  aof.open(aFileName.c_str(), ofstream::out);
  if (aof.is_open()) {
42

43
    for (int i = 0; i < LenghtDataArray; i++) {
44

Thomas Moulard's avatar
Thomas Moulard committed
45
#if 0
46
47
48
49
50
51
          aof         << 0.0*M_PI/180.0 << " "
                      << 0.0*M_PI/180.0 << " " //chest
                      << 0.0*M_PI/180.0 << " "
                      << 0.0*M_PI/180.0 << " "  // head
                      << 14.813*M_PI/180.0  << " "
                      << -10.0*M_PI/180.0 << " "
52
53
54
55
56
57
                      << 0.0*M_PI/180.0 << " " << -30.0*M_PI/180.0 << " "
                      << 0.0*M_PI/180.0 << " "  <<  0.0*M_PI/180.0 << " "
                      << 10.0*M_PI/180.0 << " "   //rarm
                      << 14.813*M_PI/180.0 << " " << 10.0*M_PI/180.0 << " "
                      << 0.0*M_PI/180.0 << " " << -30.0*M_PI/180.0 << " "
                      << 0.0*M_PI/180.0<< " "  <<  0.0*M_PI/180.0 << " "
58
                      << 10.0*M_PI/180.0<< " "  //larm
59
60
                      << -10.0*M_PI/180.0 << " " << 10.0*M_PI/180.0 << " "
                      << -10.0*M_PI/180.0 << " " << 10.0*M_PI/180.0 << " "
61
                      << -10.0*M_PI/180.0 << " " //rhand
62
63
                      << -10.0*M_PI/180.0  << " " << 10.0*M_PI/180.0 << " "
                      << -10.0*M_PI/180.0<< " " << 10.0*M_PI/180.0 << " "
64
                      << -10.0*M_PI/180.0<< " " //lhand
65
                      << endl;
Thomas Moulard's avatar
Thomas Moulard committed
66
#else
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
      aof << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " " // chest
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " " // head
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " // rarm
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " // larm
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " // rhand
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " << 0.0 * M_PI / 180.0 << " "
          << 0.0 * M_PI / 180.0 << " " // lhand
          << endl;
Thomas Moulard's avatar
Thomas Moulard committed
84
85
#endif
    }
86

87
88
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
89
90
}

91
void UpperBodyMotion::ReadDataFile(string aFileName,
92
                                   Eigen::MatrixXd &UpperBodyAngles) {
93

Thomas Moulard's avatar
Thomas Moulard committed
94
95
  std::ifstream aif;

Olivier Stasse's avatar
Olivier Stasse committed
96
97
  Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
  Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
Thomas Moulard's avatar
Thomas Moulard committed
98
99
100

  double r;

101
102
103
104
105
106
107
  aif.open(aFileName.c_str(), std::ifstream::in);
  if (aif.is_open()) {
    for (unsigned int i = 0; i < NumberRows; i++) {
      for (unsigned int j = 0; j < NumberColumns; j++) {
        aif >> r;
        UpperBodyAngles(i, j) = r;
      }
Thomas Moulard's avatar
Thomas Moulard committed
108
    }
109
110
    aif.close();
  } else
Thomas Moulard's avatar
Thomas Moulard committed
111
112
113
    std::cerr << "UpperBodyMotion - Unable to open " << aFileName << endl;
}

114
void UpperBodyMotion::WriteDataFile(string aFileName,
115
                                    Eigen::MatrixXd &UpperBodyAngles) {
Thomas Moulard's avatar
Thomas Moulard committed
116
  ofstream aof;
Olivier Stasse's avatar
Olivier Stasse committed
117
118
  Eigen::MatrixXd::Index NumberRows = UpperBodyAngles.rows();
  Eigen::MatrixXd::Index NumberColumns = UpperBodyAngles.cols();
Thomas Moulard's avatar
Thomas Moulard committed
119

120
121
  aof.open(aFileName.c_str(), ofstream::out);
  if (aof.is_open()) {
122

123
124
125
126
127
128
129
130
    for (unsigned int i = 0; i < NumberRows; i++) {
      for (unsigned int j = 0; j < NumberColumns; j++) {
        aof << UpperBodyAngles(i, j) << "\t";
      }
      aof << endl;
    }
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
131
}