diff --git a/include/sot/tools/simpleseqplay.hh b/include/sot/tools/simpleseqplay.hh index ade0f3f062fbcea34da7211cf28a42759f739ec0..107f1ebf54d6f68c4faab5f391414acec2a99746 100644 --- a/include/sot/tools/simpleseqplay.hh +++ b/include/sot/tools/simpleseqplay.hh @@ -38,8 +38,6 @@ class SimpleSeqPlay : public dg::Entity { void load(const std::string& filename); void start(); - void hold(); - void unhold(); virtual std::string getDocString() const; bool waiting () const; @@ -53,8 +51,6 @@ class SimpleSeqPlay : public dg::Entity { // 1: going to the current position to the first position. // 2: motion in progress, 3: motion finished unsigned int state_; - std::size_t configId_prev_; - std::size_t configId_hold_; int startTime_; std::vector<dg::Vector> posture_; @@ -67,10 +63,6 @@ class SimpleSeqPlay : public dg::Entity { // Number of iterations performed in state1. int it_nbs_in_state1_; - /// Boolean to hold or not the posture. - bool hold_; - /// Current shift between holding time and startTime. - }; // class SimpleSeqPlay } // namespace tools } // namespace sot diff --git a/src/simpleseqplay.cc b/src/simpleseqplay.cc index cc8378ab821ab14de0116b7928b222bca9278dd5..254acf0bcb30447a8468a03aed6bd1974e545ee3 100644 --- a/src/simpleseqplay.cc +++ b/src/simpleseqplay.cc @@ -40,7 +40,6 @@ SimpleSeqPlay::SimpleSeqPlay(const std::string& name) "SimpleSeqPlay(" + name + ")::output(vector)::posture"), currentPostureSIN_(NULL, "SimpleSeqPlay(" + name + ")::input(vector)::currentPosture"), state_(0), - configId_prev_(0), startTime_(0), posture_(), time_(0), @@ -67,8 +66,6 @@ SimpleSeqPlay::SimpleSeqPlay(const std::string& name) addCommand("load", makeCommandVoid1(*this, &SimpleSeqPlay::load, docstring)); addCommand("start", makeCommandVoid0(*this, &SimpleSeqPlay::start, docCommandVoid0("Start motion"))); - addCommand("hold", makeCommandVoid0(*this, &SimpleSeqPlay::hold, docCommandVoid0("Hold motion"))); - addCommand("unhold", makeCommandVoid0(*this, &SimpleSeqPlay::unhold, docCommandVoid0("Restart motion"))); docstring = "Set the time between the robot current pose and the starting of the buffer \n"; @@ -145,8 +142,9 @@ dg::Vector& SimpleSeqPlay::computePosture(dg::Vector& pos, int t) { if (posture_.size() == 0) { throw std::runtime_error("SimpleSeqPlay posture: Signals not initialized. read files first."); } + // Going to the first position - else if (state_ == 1) { + if (state_ == 1) { // Compute the difference between current posture and desired one. dg::Vector deltapos = posture_[0] - currentPostureSIN_.access(t); @@ -167,7 +165,6 @@ dg::Vector& SimpleSeqPlay::computePosture(dg::Vector& pos, int t) { // Tries to go through the list of postures. else if (state_ == 2) { configId = t - startTime_; - if (hold_) configId_hold_ = configId; if (configId == posture_.size() - 1) { state_ = 3; } @@ -175,17 +172,9 @@ dg::Vector& SimpleSeqPlay::computePosture(dg::Vector& pos, int t) { configId = posture_.size() - 1; } pos = posture_[configId]; - configId_prev_ = configId; return pos; } -void SimpleSeqPlay::hold() { - hold_ = true; - configId_hold_ = configId_prev_; -} - -void SimpleSeqPlay::unhold() { hold_ = false; } - bool SimpleSeqPlay::waiting () const { return state_ == 0; } bool SimpleSeqPlay::initializing () const { return state_ == 1; } bool SimpleSeqPlay::executing () const { return state_ == 2; }