GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/simpleseqplay.cc Lines: 71 87 81.6 %
Date: 2023-01-29 11:05:01 Branches: 79 194 40.7 %

Line Branch Exec Source
1
//
2
// Copyright (C) 2012, 2013, 2017 LAAS-CNRS
3
//
4
// From Author: Florent Lamiraux, Mehdi Benallegue,
5
// Author: Olivier Stasse, Rohan Budhiraja
6
// Simple sequence player just playing back a set of poses.
7
//
8
9
#include "sot/tools/simpleseqplay.hh"
10
11
#include <dynamic-graph/command-bind.h>
12
#include <dynamic-graph/command-direct-getter.h>
13
#include <dynamic-graph/command-direct-setter.h>
14
#include <dynamic-graph/command-setter.h>
15
16
#include <boost/tokenizer.hpp>
17
#include <iostream>
18
#include <stdexcept>
19
#include <vector>
20
21
namespace dynamicgraph {
22
namespace sot {
23
using command::docDirectGetter;
24
using command::docDirectSetter;
25
using command::makeDirectGetter;
26
using command::makeDirectSetter;
27
28
namespace tools {
29
using dynamicgraph::Entity;
30
using dynamicgraph::command::docCommandVoid0;
31
using dynamicgraph::command::docCommandVoid1;
32
using dynamicgraph::command::makeCommandVoid0;
33
using dynamicgraph::command::makeCommandVoid1;
34
35
1
SimpleSeqPlay::SimpleSeqPlay(const std::string& name)
36
    : Entity(name),
37
      firstSINTERN(NULL, sotNOSIGNAL,
38
2
                   "SimpleSeqPlay(" + name + ")::intern(dummy)::init"),
39
      postureSOUT_(boost::bind(&SimpleSeqPlay::computePosture, this, _1, _2),
40
                   currentPostureSIN_,
41
2
                   "SimpleSeqPlay(" + name + ")::output(vector)::posture"),
42
      currentPostureSIN_(
43
2
          NULL, "SimpleSeqPlay(" + name + ")::input(vector)::currentPosture"),
44
      state_(0),
45
      startTime_(0),
46
      posture_(),
47
      time_(0),
48
      dt_(0.001),
49
      time_to_start_(3.0),
50





7
      it_nbs_in_state1_(0) {
51
1
  firstSINTERN.setConstant(0);
52

1
  signalRegistration(postureSOUT_);
53

1
  signalRegistration(currentPostureSIN_);
54
55
  std::string docstring =
56
      "Load files describing a whole-body motion as reference feature "
57
      "trajectories\n"
58
      "\n"
59
      "  Input:\n"
60
      "    - string filename without extension\n"
61
      "\n"
62
      "  Data is read from the following files:\n"
63
      "    - posture from file \"filename.posture\",\n"
64
      "  The file should contain one column for time and as many columns as"
65
      " required\n"
66
      "  depending of degree-of-freedrom.\n"
67
1
      "\n";
68

1
  addCommand("load", makeCommandVoid1(*this, &SimpleSeqPlay::load, docstring));
69
70

1
  addCommand("start", makeCommandVoid0(*this, &SimpleSeqPlay::start,
71

2
                                       docCommandVoid0("Start motion")));
72
73
  docstring =
74
      "Set the time between the robot current pose and the starting of the "
75
1
      "buffer \n";
76
77

1
  addCommand("setTimeToStart",
78
1
             makeDirectSetter(
79
                 *this, &time_to_start_,
80

2
                 docDirectSetter("Time to start of the buffer", "double")));
81
1
}
82
83
1
void SimpleSeqPlay::load(const std::string& filename) {
84
1
  state_ = 0;
85
86
  using boost::escaped_list_separator;
87
  typedef boost::tokenizer<escaped_list_separator<char> > tokenizer_t;
88
2
  std::string line;
89
2
  std::string fn;
90
91
2
  std::ifstream file;
92
1
  unsigned int lineNumber = 0;
93
1
  int postureSize = -2;
94
1
  fn = filename + ".posture";
95
  // Open file
96
1
  file.open(fn.c_str());
97

1
  if (!file.is_open()) {
98
    throw std::runtime_error(std::string("Failed to open file ") + fn);
99
  }
100
101
1
  posture_.clear();
102
103
  // Read posture
104

6201
  while (file.good()) {
105
6201
    std::getline(file, line);
106
6201
    ++lineNumber;
107

6201
    tokenizer_t tok(line, escaped_list_separator<char>('\\', ' ', '\"'));
108
6201
    std::vector<double> components;
109


210801
    for (tokenizer_t::iterator it = tok.begin(); it != tok.end(); ++it) {
110

204600
      components.push_back(atof(it->c_str()));
111
    }
112
6201
    if (components.size() == 0) {
113
1
      break;
114
    }
115
6200
    if (postureSize == -2) {
116
1
      postureSize = static_cast<int>(components.size() - 1);
117
    } else {
118
6199
      if (postureSize != static_cast<int>(components.size()) - 1) {
119
        std::ostringstream oss;
120
        oss << fn << ", line " << lineNumber << ": config of size "
121
            << components.size() - 1 << ". Expecting " << postureSize << ".";
122
        throw std::runtime_error(oss.str());
123
      }
124
    }
125
12400
    dg::Vector config(static_cast<unsigned>(components.size() - 1));
126
204600
    for (unsigned i = 1; i < components.size(); ++i) {
127
198400
      config(i - 1) = components[i];
128
    }
129
6200
    posture_.push_back(config);
130
  }
131
1
  file.close();
132
1
}
133
134
1
void SimpleSeqPlay::start() {
135
1
  if (state_ == 0) {
136
1
    state_ = 1;
137
1
    startTime_ = postureSOUT_.getTime();
138
  }
139
1
}
140
141
6200
dg::Vector& SimpleSeqPlay::computePosture(dg::Vector& pos, int t) {
142
  std::size_t configId;
143
  // If we are still waiting to start
144
6200
  if (state_ == 0) {
145
    // return the current posture.
146
    pos = currentPostureSIN_.access(t);
147
    return pos;
148
  }
149
6200
  if (posture_.size() == 0) {
150
    throw std::runtime_error(
151
        "SimpleSeqPlay posture: Signals not initialized. read files first.");
152
  }
153
154
  // Going to the first position
155
6200
  if (state_ == 1) {
156
    // Compute the difference between current posture and desired one.
157

1
    dg::Vector deltapos = posture_[0] - currentPostureSIN_.access(t);
158
159
    // If sufficiently closed to the first posture of the seqplay.
160

1
    if ((deltapos.norm() < 1e-4) ||
161
        (((dt_ + 1) * it_nbs_in_state1_) > time_to_start_)) {
162
      // Switch to the next state.
163
1
      state_ = 2;
164
1
      startTime_ = postureSOUT_.getTime();
165
1
      pos = posture_[0];
166
    } else {
167
      // Tries to go closer to the first posture.
168
      deltapos = (deltapos * dt_) / (time_to_start_ - dt_ * it_nbs_in_state1_);
169
      pos = currentPostureSIN_.access(t) + deltapos;
170
      it_nbs_in_state1_++;
171
    }
172
1
    return pos;
173
  }
174
  // Tries to go through the list of postures.
175
6199
  else if (state_ == 2) {
176
6199
    configId = t - startTime_;
177
6199
    if (configId == posture_.size() - 1) {
178
1
      state_ = 3;
179
    }
180
  } else {
181
    configId = posture_.size() - 1;
182
  }
183
6199
  pos = posture_[configId];
184
6199
  return pos;
185
}
186
187
1
bool SimpleSeqPlay::waiting() const { return state_ == 0; }
188
1
bool SimpleSeqPlay::initializing() const { return state_ == 1; }
189
6199
bool SimpleSeqPlay::executing() const { return state_ == 2; }
190
1
bool SimpleSeqPlay::finished() const { return state_ == 3; }
191
192
std::string SimpleSeqPlay::getDocString() const {
193
  return "Provide joint references for a whole-body motion\n"
194
         "\n"
195
         "  The reference trajectories of the joints are loaded from the file\n"
196
         "  using command load.\n"
197
         "\n"
198
         "  To use this entity,\n"
199
         "    1. call method load,\n"
200
         "    2. plug reference signals into robot signals and\n"
201
         "    3. call method start.\n"
202
         "  Warning: pluging signals before loading trajectories will fail.\n";
203
}
204
205
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleSeqPlay, "SimpleSeqPlay");
206
}  // namespace tools
207
}  // namespace sot
208
}  // namespace dynamicgraph