Head

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 %


//
// Copyright (C) 2012, 2013, 2017 LAAS-CNRS
//
// From Author: Florent Lamiraux, Mehdi Benallegue,
// Author: Olivier Stasse, Rohan Budhiraja
// Simple sequence player just playing back a set of poses.
//

#include "sot/tools/simpleseqplay.hh"

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
#include <dynamic-graph/command-setter.h>

#include <boost/tokenizer.hpp>
#include <iostream>
#include <stdexcept>
#include <vector>

namespace dynamicgraph {
namespace sot {
using command::docDirectGetter;
using command::docDirectSetter;
using command::makeDirectGetter;
using command::makeDirectSetter;

namespace tools {
using dynamicgraph::Entity;
using dynamicgraph::command::docCommandVoid0;
using dynamicgraph::command::docCommandVoid1;
using dynamicgraph::command::makeCommandVoid0;
using dynamicgraph::command::makeCommandVoid1;

SimpleSeqPlay::SimpleSeqPlay(const std::string& name)
    : Entity(name),
      firstSINTERN(NULL, sotNOSIGNAL,
                   "SimpleSeqPlay(" + name + ")::intern(dummy)::init"),
      postureSOUT_(boost::bind(&SimpleSeqPlay::computePosture, this, _1, _2),
                   currentPostureSIN_,
                   "SimpleSeqPlay(" + name + ")::output(vector)::posture"),
      currentPostureSIN_(
          NULL, "SimpleSeqPlay(" + name + ")::input(vector)::currentPosture"),
      state_(0),
      startTime_(0),
      posture_(),
      time_(0),
      dt_(0.001),
      time_to_start_(3.0),
      it_nbs_in_state1_(0) {
  firstSINTERN.setConstant(0);
  signalRegistration(postureSOUT_);
  signalRegistration(currentPostureSIN_);

  std::string docstring =
      "Load files describing a whole-body motion as reference feature "
      "trajectories\n"
      "\n"
      "  Input:\n"
      "    - string filename without extension\n"
      "\n"
      "  Data is read from the following files:\n"
      "    - posture from file \"filename.posture\",\n"
      "  The file should contain one column for time and as many columns as"
      " required\n"
      "  depending of degree-of-freedrom.\n"
      "\n";
  addCommand("load", makeCommandVoid1(*this, &SimpleSeqPlay::load, docstring));

  addCommand("start", makeCommandVoid0(*this, &SimpleSeqPlay::start,
                                       docCommandVoid0("Start motion")));

  docstring =
      "Set the time between the robot current pose and the starting of the "
      "buffer \n";

  addCommand("setTimeToStart",
             makeDirectSetter(
                 *this, &time_to_start_,
                 docDirectSetter("Time to start of the buffer", "double")));
}

void SimpleSeqPlay::load(const std::string& filename) {
  state_ = 0;

  using boost::escaped_list_separator;
  typedef boost::tokenizer<escaped_list_separator<char> > tokenizer_t;
  std::string line;
  std::string fn;

  std::ifstream file;
  unsigned int lineNumber = 0;
  int postureSize = -2;
  fn = filename + ".posture";
  // Open file
  file.open(fn.c_str());
  if (!file.is_open()) {
    throw std::runtime_error(std::string("Failed to open file ") + fn);
  }

  posture_.clear();

  // Read posture
  while (file.good()) {
    std::getline(file, line);
    ++lineNumber;
    tokenizer_t tok(line, escaped_list_separator<char>('\\', ' ', '\"'));
    std::vector<double> components;
    for (tokenizer_t::iterator it = tok.begin(); it != tok.end(); ++it) {
      components.push_back(atof(it->c_str()));
    }
    if (components.size() == 0) {
      break;
    }
    if (postureSize == -2) {
      postureSize = static_cast<int>(components.size() - 1);
    } else {
      if (postureSize != static_cast<int>(components.size()) - 1) {
        std::ostringstream oss;
        oss << fn << ", line " << lineNumber << ": config of size "
            << components.size() - 1 << ". Expecting " << postureSize << ".";
        throw std::runtime_error(oss.str());
      }
    }
    dg::Vector config(static_cast<unsigned>(components.size() - 1));
    for (unsigned i = 1; i < components.size(); ++i) {
      config(i - 1) = components[i];
    }
    posture_.push_back(config);
  }
  file.close();
}

void SimpleSeqPlay::start() {
  if (state_ == 0) {
    state_ = 1;
    startTime_ = postureSOUT_.getTime();
  }
}

dg::Vector& SimpleSeqPlay::computePosture(dg::Vector& pos, int t) {
  std::size_t configId;
  // If we are still waiting to start
  if (state_ == 0) {
    // return the current posture.
    pos = currentPostureSIN_.access(t);
    return pos;
  }
  if (posture_.size() == 0) {
    throw std::runtime_error(
        "SimpleSeqPlay posture: Signals not initialized. read files first.");
  }

  // Going to the first position
  if (state_ == 1) {
    // Compute the difference between current posture and desired one.
    dg::Vector deltapos = posture_[0] - currentPostureSIN_.access(t);

    // If sufficiently closed to the first posture of the seqplay.
    if ((deltapos.norm() < 1e-4) ||
        (((dt_ + 1) * it_nbs_in_state1_) > time_to_start_)) {
      // Switch to the next state.
      state_ = 2;
      startTime_ = postureSOUT_.getTime();
      pos = posture_[0];
    } else {
      // Tries to go closer to the first posture.
      deltapos = (deltapos * dt_) / (time_to_start_ - dt_ * it_nbs_in_state1_);
      pos = currentPostureSIN_.access(t) + deltapos;
      it_nbs_in_state1_++;
    }
    return pos;
  }
  // Tries to go through the list of postures.
  else if (state_ == 2) {
    configId = t - startTime_;
    if (configId == posture_.size() - 1) {
      state_ = 3;
    }
  } else {
    configId = posture_.size() - 1;
  }
  pos = posture_[configId];
  return pos;
}

bool SimpleSeqPlay::waiting() const { return state_ == 0; }
bool SimpleSeqPlay::initializing() const { return state_ == 1; }
bool SimpleSeqPlay::executing() const { return state_ == 2; }
bool SimpleSeqPlay::finished() const { return state_ == 3; }

std::string SimpleSeqPlay::getDocString() const {
  return "Provide joint references for a whole-body motion\n"
         "\n"
         "  The reference trajectories of the joints are loaded from the file\n"
         "  using command load.\n"
         "\n"
         "  To use this entity,\n"
         "    1. call method load,\n"
         "    2. plug reference signals into robot signals and\n"
         "    3. call method start.\n"
         "  Warning: pluging signals before loading trajectories will fail.\n";
}

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleSeqPlay, "SimpleSeqPlay");
}  // namespace tools
}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

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