YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
controlThread.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include <yarp/os/Log.h>
7#include <yarp/os/LogStream.h>
8
9#include "robotDriver.h"
10#include "robotAction.h"
11#include "controlThread.h"
12
14{
15 std::lock_guard<std::mutex> lg(m_mtx);
16 return m_status;
17}
18
19bool ControlThread::action_getname(std::string& name)
20{
21 std::lock_guard<std::mutex> lg(m_mtx);
22 name = m_current_action->action_name;
23 return true;
24}
25
27{
28 std::lock_guard<std::mutex> lg(m_mtx);
29 m_status = ACTION_STOP;
30 //the clock is paused in ACTION_STOP
31 return true;
32}
33
35{
36 std::lock_guard<std::mutex> lg(m_mtx);
37 m_current_action->print();
38 return true;
39}
40
42{
43 std::lock_guard<std::mutex> lg(m_mtx);
44 m_status = ACTION_RESET;
45 m_current_action->current_frame = 0;
46 //the clock is resetted in ACTION_RESET
47 return true;
48}
49
51{
52 std::lock_guard<std::mutex> lg(m_mtx);
53 if (m_current_action->current_frame == 0)
54 {
55 m_status = ACTION_START;
56 //if the action is just started the clock will be started in ACTION_START
57 }
58 else
59 {
60 m_status = ACTION_RUNNING;
61 //if we are resuming a paused action, start the timer previously stopped by aCTION_STOP
63 }
64 m_current_action->forever = true;
65 return true;
66}
67
69{
70 std::lock_guard<std::mutex> lg(m_mtx);
71 if (m_current_action->current_frame == 0)
72 {
73 m_status = ACTION_START;
74 //if the action is just started the clock will be started in ACTION_START
75 }
76 else
77 {
78 m_status = ACTION_RUNNING;
79 //if we are resuming a paused action, start the timer previously stopped by aCTION_STOP
81 }
82 m_current_action->forever = false;
83 return true;
84}
85
87{
88 std::lock_guard<std::mutex> lg(m_mtx);
89 m_current_action = action;
90 m_current_driver = driver;
91 m_status = ACTION_IDLE;
92 m_current_action->current_frame = 0;
94 return true;
95}
96
98{
99 std::lock_guard<std::mutex> lg(m_mtx);
100 m_current_action->speed_factor = factor;
101 //also reset the action to avoid issue if it is playing..
102 m_status = ACTION_RESET;
103 m_current_action->current_frame = 0;
104 return true;
105}
106
108{
109 std::lock_guard<std::mutex> lg(m_mtx);
110 m_current_action->interpolate_action_frames(value);
111
112 //also reset the action to avoid issue if it is playing..
113 m_status = ACTION_RESET;
114 m_current_action->current_frame = 0;
115 return true;
116}
117
118ControlThread::ControlThread(std::string name, double period): PeriodicThread(period)
119{
120 yInfo() << "Control thread period set to: " << period;
121 m_module_name = name;
122}
123
125{
126 m_port_command_out.interrupt();
127 m_port_command_out.close();
128 m_port_command_joints.interrupt();
129 m_port_command_joints.close();
130}
131
133{
134 if (!m_port_command_out.open(std::string("/")+m_module_name+"/port_command_out:o"))
135 {
136 return false;
137 }
138
139 if (!m_port_command_joints.open(std::string("/")+m_module_name+"/port_joints:o"))
140 {
141 return false;
142 }
143 return true;
144}
145
146bool ControlThread::execute_joint_command(int frame_id)
147{
148 if (!m_current_driver) return false;
149 if (!m_enable_execute_joint_command) return true;
150
151 double *ll = m_current_action->action_frames_vector[frame_id].q_joints.data();
152 size_t nj = m_current_action->get_njoints();
153
154 for (size_t j = 0; j < nj; j++)
155 {
156 m_current_driver->setPosition((int)j, ll[j]);
157 }
158 return true;
159}
160
161void ControlThread::compute_and_send_command(int frame_id)
162{
163 //prepare the output command
164 yarp::os::Bottle& bot = m_port_command_out.prepare();
165 bot.clear();
166 bot.addInt32((int)m_current_action->action_frames_vector[frame_id].counter);
167 bot.addFloat64(m_current_action->action_frames_vector[frame_id].time);
168
169 //send the output command
170 m_port_command_out.write();
171
172 //execute the command
173 if (!execute_joint_command(frame_id))
174 {
175 yError("failed to execute command");
176 }
177
178 //quick reads the current position
179 std::vector<double> encs;
180 if (m_current_driver)
181 {
182 size_t nj = m_current_action->get_njoints();
183 encs.resize(nj);
184 for (size_t j = 0; j < nj; j++)
185 {
186 m_current_driver->getEncoder((int)j, &encs[j]);
187 }
188 }
189 else
190 {
191 //invalid driver
192 yError("Critical error: invalid driver");
193 }
194
195 //send the joints angles on debug port
196 double *ll = m_current_action->action_frames_vector[frame_id].q_joints.data();
197 yarp::os::Bottle& bot2 = this->m_port_command_joints.prepare();
198 bot2.clear();
199 bot2.addInt32((int)m_current_action->action_frames_vector[frame_id].counter);
200 bot2.addFloat64(m_current_action->action_frames_vector[frame_id].time);
201 size_t nj = m_current_action->get_njoints();
202 bot2.addString("commands:");
203 for (size_t ix=0;ix<nj;ix++)
204 {
205 bot2.addFloat64(ll[ix]);
206 }
207 bot2.addString("encoders:");
208 for (size_t ix=0;ix<nj;ix++)
209 {
210 bot2.addFloat64(encs[ix]);
211 }
212 this->m_port_command_joints.write();
213}
214
215size_t getCurrentFrame(double elapsed, std::deque<action_frame>& dq)
216{
217 //example
218 // 0 0.0 > 1.1 ? no, continue
219 // 1 0.5 > 1.1 ? no, continue
220 // 2 1.0 > 1.1 ? no, continue
221 // 3 1.5 > 1.1 ? yes, return 3
222 // 4 2.0
223
224 size_t i = 0;
225 for (i = 0; i < dq.size(); i++)
226 {
227 if (dq[i].time > elapsed)
228 return i;
229 }
230 return i;
231}
232
234{
235 std::lock_guard<std::mutex> lck(m_mtx);
236
237 size_t nj = m_current_action->get_njoints();
238
239 if (this->m_status == ACTION_IDLE)
240 {
241 // do nothing
242 //yDebug() << "ACTION_IDLE";
243 }
244 else if (this->m_status == ACTION_STOP)
245 {
246 yInfo() << "ACTION_STOP";
248 this->m_status = ACTION_IDLE;
249 }
250 else if (this->m_status == ACTION_RESET)
251 {
252 yInfo() << "ACTION_RESET";
253
254 for (size_t j = 0; j < nj; j++)
255 {
257 }
258
259 m_current_driver->loadRefVelocities();
260
262 this->m_status = ACTION_IDLE;
263 }
264 else if (this->m_status == ACTION_RUNNING)
265 {
266 size_t last_frame = m_current_action->action_frames_vector.size();
267 if (last_frame == 0)
268 {
269 yError("ACTION_RUNNING: sequence empty!");
270 this->m_status = ACTION_RESET;
271 return;
272 }
273
274 //if it's not the last frame
275 double curr_frame_time = m_clock.getElapsedTime() * m_current_action->speed_factor;
276 size_t newcurrframe = getCurrentFrame(curr_frame_time, m_current_action->action_frames_vector);
277 m_current_action->current_frame = newcurrframe;
278
279 // not the last frame
280 if (m_current_action->current_frame < last_frame - 1)
281 {
282 compute_and_send_command((int)m_current_action->current_frame);
283 if (m_verbose) {yDebug("ACTION_RUNNING: Executing action: %4zd/%4zd (%.3fs)", m_current_action->current_frame , last_frame, m_clock.getElapsedTime());}
284 }
285 else //the action is complete
286 {
287 if (m_current_action->forever)
288 {
289 yInfo("ACTION_RUNNING: sequence completed in: %.3f s, restarting", m_clock.getElapsedTime());
290 m_current_action->current_frame=0;
293 }
294 else
295 {
296 yInfo("ACTION_RUNNING: sequence completed in: %.3f s", m_clock.getElapsedTime());
297 for (size_t j = 0; j < nj; j++)
298 {
300 }
301 this->m_status=ACTION_IDLE;
302 }
303 m_current_driver->loadRefVelocities();
304 }
305 }
306 else if (this->m_status == ACTION_START)
307 {
308 m_current_driver->storeRefVelocities();
309
310 if (m_current_action->action_frames_vector.size() > 0)
311 {
312 double *ll = m_current_action->action_frames_vector[0].q_joints.data();
313 size_t nj = m_current_action->get_njoints();
314
315 //first go to first frame in standard position mode
316 yDebug() << "ACTION_START: switch to position mode";
317 for (size_t j = 0; j < nj; j++)
318 {
320 }
322 for (size_t j = 0; j < nj; j++)
323 {
324 m_current_driver->positionMove((int)j, ll[j]);
325 }
326
327 //check if it reached the position within the desired tolerance
328 yInfo() << "ACTION_START: going to start position";
329 double enc= 0.0;
330 bool check = true;
331 std::vector<double> err(nj);
332 double move_start_time = yarp::os::Time::now();
333 do
334 {
335 check = true;
336 for (size_t j = 0; j < nj; j++)
337 {
338 m_current_driver->getEncoder((int)j, &enc);
339 err[j] = fabs(enc - ll[j]);
340 check &= (err[j] < m_current_action->m_tolerances[j]);
341 }
343 if (check)
344 {
345 yInfo() << "ACTION_START: start position reached successfully";
346 break;
347 }
348 if (yarp::os::Time::now() - move_start_time > m_home_position_timeout)
349 {
350 yWarning() << "ACTION_START: timeout while trying to reach start position";
351 for (size_t j = 0; j < nj; j++)
352 {
353 yWarning() << "j: "<< j << " err:" << err[j] << " tol:" << m_current_action->m_tolerances[j];
354 }
355 break;
356 }
357 } while (1);
358
359 //check after initial position move
360 if (check == false)
361 {
362 yError() << "ACTION_START: unable to reach start position!";
363 if (m_home_position_strict_check_enabled)
364 {
365 //very strict behavior! if your are controlling fingers, you will probably end here
366 this->m_status = ACTION_STOP;
367 yError() << "ACTION_START: m_home_position_strict_check_enabled, stopping!";
368 m_current_driver->loadRefVelocities();
369 return;
370 }
371 }
372 for (int j = 0; j <nj; j++)
373 {
375 m_current_driver->getControlMode(j, mode);
378 )
379 {
380 this->m_status = ACTION_STOP;
381 yError() << "ACTION_START: control mode check failed!";
382 m_current_driver->loadRefVelocities();
383 return;
384 }
385 }
386
387 //switch to position direct mode
388 yDebug() << "ACTION_START: switch to position direct mode";
389 for (int j = 0; j <nj; j++)
390 {
392 }
394
395 //go to step 0
396 yDebug() << "ACTION_START: moving to step 0 in direct mode";
397 compute_and_send_command(0);
398
399 //ready to go
400 this->m_status = ACTION_RUNNING;
402 yInfo() << "ACTION_START: sequence started";
403 }
404 else
405 {
406 yWarning("no sequence in memory");
408 this->m_status = ACTION_STOP;
409 }
410 }
411 else
412 {
413 yError() << "unknown current_status";
414 }
415}
416
418{
419 if (tolerance > 0)
420 {
421 for (size_t i=0; i< m_current_action->get_njoints(); i++)
422 {
423 m_current_action->m_tolerances[i] = tolerance;
424 }
425 }
426}
427
429{
430 if (timeout > 0)
431 {
432 m_home_position_timeout = timeout;
433 }
434}
435
437{
438 m_home_position_strict_check_enabled=enable;
439}
440
442{
443 if (m_current_driver)
444 {
445 m_current_driver->setTrajectoryTime(t);
446 }
447}
#define yInfo(...)
Definition Log.h:319
#define yError(...)
Definition Log.h:361
#define yDebug(...)
Definition Log.h:275
#define yWarning(...)
Definition Log.h:340
double getElapsedTime()
bool threadInit() override
Initialization method.
bool action_getname(std::string &name)
void setPositionStrictCheck(bool enable)
bool m_enable_execute_joint_command
void setInitialMoveTime(double t)
void run() override
Loop function.
action_status_enum getStatus()
bool action_resample(double value)
bool action_setSpeedFactor(double factor=1)
void setPositionTolerance(double tolerance)
bool action_change(action_class *action, robotDriver *driver)
ControlThread(std::string name, double period)
ControlClock m_clock
void setPositionTimeout(double timeloops)
void interpolate_action_frames(double timestep)
std::string action_name
Definition robotAction.h:48
size_t get_njoints()
size_t current_frame
Definition robotAction.h:50
double speed_factor
Definition robotAction.h:52
std::vector< double > m_tolerances
Definition robotAction.h:53
std::deque< action_frame > action_frames_vector
Definition robotAction.h:54
bool setTrajectoryTime(double t)
bool loadRefVelocities()
bool setPosition(int j, double ref)
bool setControlMode(const int j, const yarp::dev::SelectableControlModeEnum mode)
bool getEncoder(int j, double *v)
bool storeRefVelocities()
bool getControlMode(const int j, yarp::dev::ControlModeEnum &mode)
bool positionMove(int j, double ref)
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:65
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition Bottle.cpp:158
void clear()
Empties the bottle of any objects it contains.
Definition Bottle.cpp:121
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition Bottle.cpp:140
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition Bottle.cpp:176
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
size_t getCurrentFrame(double elapsed, std::deque< action_frame > &dq)
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
action_status_enum
Definition robotAction.h:18
@ ACTION_STOP
Definition robotAction.h:22
@ ACTION_IDLE
Definition robotAction.h:19
@ ACTION_RESET
Definition robotAction.h:23
@ ACTION_START
Definition robotAction.h:20
@ ACTION_RUNNING
Definition robotAction.h:21