iCub-main
WholeBodyPlayerModule.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
45 #include <yarp/os/BufferedPort.h>
46 #include <yarp/os/LogStream.h>
47 #include <yarp/os/RFModule.h>
48 #include <yarp/dev/IPositionDirect.h>
49 #include <yarp/dev/IPositionControl.h>
50 #include <yarp/dev/IControlMode.h>
51 #include <yarp/dev/IEncoders.h>
52 #include <yarp/dev/IControlLimits.h>
53 #include <yarp/dev/PolyDriver.h>
54 
55 #include <atomic>
56 #include <vector>
57 #include <cmath>
58 #include <memory>
59 #include <mutex>
60 #include <yarp/os/RpcClient.h>
61 
62 constexpr double tolerance = 5.0; //degrees
63 
64 enum class state : std::uint8_t {
65  ok = 0,
66  error,
68 };
69 
70 class ReplayPort : public yarp::os::BufferedPort<yarp::os::Bottle>
71 {
72 public:
73  ReplayPort(std::string partName, yarp::dev::IPositionDirect* iPosDir,
74  yarp::dev::IEncoders* iEnc, yarp::dev::IControlMode* iCM,
75  yarp::dev::IPositionControl* iPosControl,
76  yarp::dev::IControlLimits* iControlLimits, bool simulator=false) :
77  m_partName(std::move(partName)),
78  m_posDir(iPosDir),
79  m_enc(iEnc),
80  m_CM(iCM),
81  m_posControl(iPosControl),
82  m_controlLimits(iControlLimits),
83  m_simulator(simulator)
84 
85  {
86  if (m_posDir && m_enc && m_CM && m_controlLimits) {
87  // Get num of axes
88  m_posDir->getAxes(&m_numAxes);
89  // Set in position direct
90  std::vector<int> cms (m_numAxes, VOCAB_CM_POSITION_DIRECT);
91  m_CM->setControlModes(cms.data());
92  // Allocate vectors
93  m_currState.resize(m_numAxes);
94  m_nextState.resize(m_numAxes);
95  max.resize(m_numAxes);
96  min.resize(m_numAxes);
97  for (size_t i = 0; i<m_numAxes; i++) {
98  m_controlLimits->getLimits(i,&min[i],&max[i]);
99  }
100  }
101  yarp::os::BufferedPort<yarp::os::Bottle>::useCallback();
102  yarp::os::BufferedPort<yarp::os::Bottle>::setStrict(true);
103  m_isArm = m_partName.find("arm") != std::string::npos;
104  }
105 
106  using yarp::os::TypedReaderCallback<yarp::os::Bottle>::onRead;
107  void onRead(yarp::os::Bottle& datum) override
108  {
109  if (m_state==state::ok && m_posDir && !datum.isNull()) {
110 
111  bool ok = m_enc->getEncoders(m_currState.data());
112  m_mutex.lock();
113  for (size_t i=0; i<datum.size(); i++) {
114  m_nextState[i] = datum.get(i).asFloat64();
115  if (m_nextState[i]<min[i] || m_nextState[i]> max[i]) {
116  yWarning()<<"ReplayPort: trying to move joint"<<i<<"of"<<m_partName<<" to "<<m_nextState[i]<<", it exceeds the limits, skipping...";
117  continue;
118  }
119  auto delta = std::fabs(m_nextState[i] - m_currState[i]);
120  ok &= delta < tolerance; // TODO improve the check calculating the distance between the 2 vector !!!
121  if (!ok && !m_simulator && (!m_isArm || i < 5)) { // 5 is for ignoring the hands in the security check
122  yWarning()<<"ReplayPort: joint"<<i<<"of"<<m_partName<<"is too far to the target position";
123  yWarning()<<"Desired: "<<datum.get(i).asFloat64()<<"current: "<<m_currState[i]
124  <<"delta: "<<delta<<"Trying to reach "
125  "it in Position Control, "
126  "the playback will be paused";
128  ok = true;
129 
130  }
131  }
132  m_mutex.unlock();
133  if (m_state == state::ok) {
134  m_posDir->setPositions(m_nextState.data());
135  }
136  }
137  }
138 
140  bool ok = true;
141  std::vector<int> cms (m_numAxes, VOCAB_CM_POSITION);
142  ok &= m_CM->setControlModes(cms.data());
143 
144  if (!ok) {
146  return ok;
147  }
148 
149  m_mutex.lock();
150  ok &= m_posControl->positionMove(m_nextState.data());
151  m_mutex.unlock();
152 
153  if (!ok) {
155  return ok;
156  }
157  bool done{false};
158  ok &= m_posControl->checkMotionDone(&done);
159 
160  if (!ok) {
162  return ok;
163  }
164 
165  while (!done) {
166  ok &= m_posControl->checkMotionDone(&done);
167  if (!ok) {
169  return ok;
170  }
171  yarp::os::Time::delay(0.01);
172  }
173 
174  std::vector<int> cmDir (m_numAxes, VOCAB_CM_POSITION_DIRECT);
175  ok &= m_CM->setControlModes(cmDir.data());
176  if (!ok) {
178  return ok;
179  }
180  return ok;
181  }
182 
183  std::atomic<state> m_state{state::ok};
184 private:
185  double now{0.0};
186  std::vector<double> min, max;
187  std::string m_partName{};
188  yarp::dev::IPositionDirect* m_posDir{nullptr};
189  yarp::dev::IEncoders* m_enc{nullptr};
190  yarp::dev::IPositionControl* m_posControl{nullptr};
191  yarp::dev::IControlMode* m_CM{nullptr};
192  yarp::dev::IControlLimits* m_controlLimits{nullptr};
193  std::mutex m_mutex;
194 
195 
196  std::vector<double> m_currState, m_nextState;
197  int m_numAxes{0};
198  bool m_simulator;
199  bool m_isArm{false};
200 };
201 
202 struct Replayer {
203  std::unique_ptr<ReplayPort> m_replayPort{nullptr};
204  std::unique_ptr<yarp::dev::PolyDriver> m_remoteControlBoard{nullptr};
205 
206  bool open(const std::string& robot, const std::string& part, const std::string& moduleName="wholeBodyPlayer") {
207  yarp::os::Property conf {{"device", yarp::os::Value("remote_controlboard")},
208  {"remote", yarp::os::Value("/"+robot+"/"+part)},
209  {"local", yarp::os::Value("/"+moduleName+"/"+part+"/remoteControlBoard")}};
210 
211  yarp::dev::IControlMode* iCM{nullptr};
212  yarp::dev::IPositionDirect* iPosDir{nullptr};
213  yarp::dev::IPositionControl* iPosControl{nullptr};
214  yarp::dev::IEncoders* iEnc{nullptr};
215  yarp::dev::IControlLimits* iControlLimits{nullptr};
216  bool simulator{false};
217 
218  if(robot == "icubSim")
219  simulator=true;
220 
221  m_remoteControlBoard = std::make_unique<yarp::dev::PolyDriver>();
222 
223  bool ok = m_remoteControlBoard->open(conf);
224  if (!ok) {
225  yError()<<"Replayer: failed to open the remote control board for part"<<part;
226  return false;
227  }
228  ok &= m_remoteControlBoard->view(iPosDir);
229  ok &= m_remoteControlBoard->view(iEnc);
230  ok &= m_remoteControlBoard->view(iCM);
231  ok &= m_remoteControlBoard->view(iPosControl);
232  ok &= m_remoteControlBoard->view(iControlLimits);
233 
234  if (ok)
235  {
236  m_replayPort = std::make_unique<ReplayPort>(part, iPosDir, iEnc, iCM, iPosControl,iControlLimits, simulator);
237  ok &= m_replayPort->open("/"+moduleName+"/"+part+"/state:i");
238  }
239  return ok;
240  }
241 
242  void close() {
243  m_replayPort->close();
244  m_remoteControlBoard->close();
245  }
246 };
247 
248 
249 class WholeBodyPlayerModule : public yarp::os::RFModule {
250 public:
251  ~WholeBodyPlayerModule() override = default;
252 
253  double getPeriod() override;
254 
255  bool updateModule() override;
256 
257  bool configure(yarp::os::ResourceFinder& rf) override;
258 
259  bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply) override; // NOT SURE I NEED IT. I need to call this->attach(portRpc)
260 
261  bool interruptModule() override;
262 
263  bool close() override;
264 
265 private:
266  std::vector<Replayer> m_replayerVec;
267  yarp::os::RpcClient m_rpcPort;
268  yarp::os::Bottle reqPause{"pause"}, reqPlay{"play"}, response;
269 
270 
271 };
Replayer::m_remoteControlBoard
std::unique_ptr< yarp::dev::PolyDriver > m_remoteControlBoard
Definition: WholeBodyPlayerModule.h:204
ReplayPort::m_state
std::atomic< state > m_state
Definition: WholeBodyPlayerModule.h:183
WholeBodyPlayerModule::updateModule
bool updateModule() override
Definition: WholeBodyPlayerModule.cpp:24
tolerance
constexpr double tolerance
Definition: WholeBodyPlayerModule.h:62
state
state
Definition: WholeBodyPlayerModule.h:64
state::error
@ error
Replayer::m_replayPort
std::unique_ptr< ReplayPort > m_replayPort
Definition: WholeBodyPlayerModule.h:203
python-motor-control.iEnc
iEnc
Definition: python-motor-control.py:28
Replayer::close
void close()
Definition: WholeBodyPlayerModule.h:242
ReplayPort::positionMoveFallback
bool positionMoveFallback()
Definition: WholeBodyPlayerModule.h:139
WholeBodyPlayerModule::getPeriod
double getPeriod() override
Definition: WholeBodyPlayerModule.cpp:20
ReplayPort::ReplayPort
ReplayPort(std::string partName, yarp::dev::IPositionDirect *iPosDir, yarp::dev::IEncoders *iEnc, yarp::dev::IControlMode *iCM, yarp::dev::IPositionControl *iPosControl, yarp::dev::IControlLimits *iControlLimits, bool simulator=false)
Definition: WholeBodyPlayerModule.h:73
state::ok
@ ok
WholeBodyPlayerModule::configure
bool configure(yarp::os::ResourceFinder &rf) override
Definition: WholeBodyPlayerModule.cpp:61
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
ReplayPort::onRead
void onRead(yarp::os::Bottle &datum) override
Definition: WholeBodyPlayerModule.h:107
done
bool done
Definition: main.cpp:42
ReplayPort
Definition: WholeBodyPlayerModule.h:70
Replayer::open
bool open(const std::string &robot, const std::string &part, const std::string &moduleName="wholeBodyPlayer")
Definition: WholeBodyPlayerModule.h:206
state::fatal_error
@ fatal_error
WholeBodyPlayerModule::close
bool close() override
Definition: WholeBodyPlayerModule.cpp:113
WholeBodyPlayerModule
Definition: WholeBodyPlayerModule.h:249
WholeBodyPlayerModule::interruptModule
bool interruptModule() override
Definition: WholeBodyPlayerModule.cpp:105
Replayer
Definition: WholeBodyPlayerModule.h:202
WholeBodyPlayerModule::~WholeBodyPlayerModule
~WholeBodyPlayerModule() override=default
WholeBodyPlayerModule::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply) override
Definition: WholeBodyPlayerModule.cpp:101