YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardTrajectory.cpp
Go to the documentation of this file.
2#include <yarp/os/Time.h>
3#include <cmath>
4
6{
7 m_lastUpdate = yarp::os::Time::now();
8}
9
11{
12 if (m_duration < 0)
13 return false;
14 if ((yarp::os::Time::now() - m_lastUpdate) > m_duration)
15 return true;
16 return false;
17}
18
19Watchdog::Watchdog(double expireTime)
20{
21 m_duration = expireTime;
22 m_lastUpdate = yarp::os::Time::now();
23}
24
25void Watchdog::modifyDuration(double expireTime)
26{
27 m_duration = expireTime;
28}
29
31{
32 return m_duration;
33}
34
35
36//------------------------------------------------------------------------------------------------------------------
37// RampFilter
38//------------------------------------------------------------------------------------------------------------------
39
40
42{
43 m_final_reference = 0;
44 m_current_value = 0;
45 m_step = 0;
46}
47
48void RampFilter::setReference(double ref, double step)
49{
50 std::lock_guard<std::mutex> lock(m_mutex);
51 m_final_reference = ref;
52 m_step = step;
53}
54
56{
57 std::lock_guard<std::mutex> lock(m_mutex);
58 m_final_reference = 0.0;
59 m_current_value = 0.0;
60 m_step = 0.0;
61}
62
64{
65 std::lock_guard<std::mutex> lock(m_mutex);
66 double tmp = 0;
67 double error_abs = fabs(m_final_reference - m_current_value);
68
69 if (m_final_reference > m_current_value)
70 {
71 tmp = +(m_step / 1000.0) * 1.0; //* m_controllerPeriod* _T_controller;
72 } else
73 {
74 tmp = -(m_step / 1000.0) * 1.0; //* m_controllerPeriod*_T_controller;
75 }
76
77 if (error_abs < fabs(tmp))
78 {
79 tmp = (m_final_reference - m_current_value);
80 }
81
82 m_current_value = m_current_value + tmp;
83}
84
86{
87 return m_current_value;
88}
89
90//------------------------------------------------------------------------------------------------------------------
91// TrajectoryGenerator
92//------------------------------------------------------------------------------------------------------------------
93
95 : m_trajectory_complete(true)
96 , m_speed(0)
97 , m_acceleration(0)
98 , m_joint_min(0)
99 , m_joint_max(0)
100{
101}
102
106
111
112bool TrajectoryGenerator::setLimits(double min, double max)
113{
114 m_joint_min = min;
115 m_joint_max = max;
116 return true;
117}
118
119//------------------------------------------------------------------------------------------------------------------
120// MinJerkTrajectoryGenerator
121//------------------------------------------------------------------------------------------------------------------
122
126
130
131double MinJerkTrajectoryGenerator::p_compute_p5f(double t)
132{
133 double accum = m_dx0 * t;
134 double tmp = t * t * t;
135 accum = accum + tmp * m_trajectory_coeff_c1; //(10*xfx0 - 6*dx0);
136 tmp *= t;
137 accum = accum - tmp * m_trajectory_coeff_c2; //(15*xfx0 - 8*dx0);
138 tmp *= t;
139 accum = accum + tmp * m_trajectory_coeff_c3; //(6*xfx0 - 3*dx0);
140 return accum;
141}
142
143double MinJerkTrajectoryGenerator::p_compute_p5f_vel(double t)
144{
145 double accum = -2 * m_dx0 * t - m_dx0;
146 accum = accum + (30 * m_x0 + 15 * m_dx0 - 30 * m_xf) * (t * t);
147 accum = -accum / m_tf * (t - 1) * (t - 1);
148 return accum;
149}
150
151double MinJerkTrajectoryGenerator::p_compute_current_vel()
152{
153 double a;
154
155 //(10 * (t/T)^3 - 15 * (t/T)^4 + 6 * (t/T)^5) * (x0-xf) + x0
157 return 0;
158 if (m_cur_t == 0)
159 {
160 return 0;
161 } else if (m_cur_t < 1.0 - m_step)
162 {
163 // calculate the velocity
164 a = p_compute_p5f_vel(m_cur_t);
165 return a;
166 }
167 return 0;
168}
169
170bool MinJerkTrajectoryGenerator::p_abortTrajectory(double limit)
171{
173 {
175 m_cur_t = 0;
176 m_cur_step = 0;
177 m_xf = limit;
178 } else
179 {
180 m_cur_t = 0;
181 m_cur_step = 0;
182 m_xf = limit;
183 }
184 return true;
185}
186
188{
189 std::lock_guard<std::mutex> lock(m_mutex);
190 bool ret = p_abortTrajectory(limit);
191 return ret;
192}
193
195 double final_pos,
196 double speed,
197 double acceleration,
198 std::chrono::milliseconds controller_period)
199{
200 std::lock_guard<std::mutex> lock(m_mutex);
201
202 m_controllerPeriodMilliseconds = controller_period.count();
203 double speedf = fabs(speed);
204 double dx0 = 0;
205 m_computed_reference = current_pos;
206
207 if (speed <= 0)
208 {
209 return false;
210 }
211
212 m_dx0 = p_compute_current_vel();
213 m_x0 = current_pos;
214 m_prev_a = current_pos;
215 m_xf = final_pos;
216 if (m_xf > m_joint_max)
218 else if (m_xf < m_joint_min)
220 m_speed = speed;
221 m_acceleration = acceleration; // unused
222
223 // double step = (m_trajectoryGenerationReferenceSpeed[j] / 1000.0) * m_robotRefreshPeriod *
224 // _T_controller;
225
226 m_tf = (1000 * fabs(m_xf - m_x0) / speedf) / m_controllerPeriodMilliseconds;
227 m_dx0 = m_dx0 * m_tf;
228
229 dx0 = m_dx0;
230 m_trajectory_coeff_c1 = (10 * (m_xf - m_x0) - 6 * dx0);
231 m_trajectory_coeff_c2 = (15 * (m_xf - m_x0) - 8 * dx0);
232 m_trajectory_coeff_c3 = (6 * (m_xf - m_x0) - 3 * dx0);
233
234 if (m_tf < 1 || m_tf == 0)
235 {
236 p_abortTrajectory(final_pos);
237 m_step = 0;
238 return false;
239 } else
240 {
241 m_step = 1 / m_tf;
242 }
243 m_cur_t = 0;
244 m_cur_step = 0;
245 m_trajectory_complete = false;
246
247 return true;
248}
249
250double MinJerkTrajectoryGenerator::p_computeTrajectoryStep()
251{
252 double target = 0;
253 double delta_target = 0;
254
255 // (10 * (t/T)^3 - 15 * (t/T)^4 + 6 * (t/T)^5) * (x0-xf) + x0
257 {
258 target = m_xf;
259 delta_target = target - m_prev_a;
260 m_prev_a = target;
261 // yCDebug(GAZEBOCONTROLBOARD)<<"mdone" << target;
262 return delta_target;
263 }
264
265 if (m_cur_t == 0)
266 {
267 m_cur_t += m_step;
268 m_cur_step++;
269
270 target = m_x0;
271 delta_target = target - m_prev_a;
272 m_prev_a = target;
273 // yCDebug(GAZEBOCONTROLBOARD)<<"first" ;
274 return delta_target;
275 } else if (m_cur_t < 1.0 - m_step)
276 {
277 // calculate the power factors
278 target = p_compute_p5f(m_cur_t);
279 target += m_x0;
280
281 // time
282 m_cur_t += m_step;
283 m_cur_step++;
284
285 delta_target = target - m_prev_a;
286 m_prev_a = target;
287 // yCDebug(GAZEBOCONTROLBOARD)<< delta_target;
288 return delta_target;
289 }
290
291 // yCDebug(GAZEBOCONTROLBOARD)<<"last";
293 target = m_xf;
294
295 // position reached, so step is zero
296 return 0.0;
297}
298
300{
301 std::lock_guard<std::mutex> lock(m_mutex);
302 double ret = p_computeTrajectoryStep();
303 return ret;
304}
305
306double MinJerkTrajectoryGenerator::p_computeTrajectory()
307{
308 double target = 0;
309
310 // (10 * (t/T)^3 - 15 * (t/T)^4 + 6 * (t/T)^5) * (x0-xf) + x0
312 {
313 target = m_xf;
314 m_prev_a = target;
315 // yCDebug(GAZEBOCONTROLBOARD)<<"mdone" << target;
316 return target;
317 }
318
319 if (m_cur_t == 0)
320 {
321 m_cur_t += m_step;
322 m_cur_step++;
323
324 target = m_x0;
325 m_prev_a = target;
326 // yCDebug(GAZEBOCONTROLBOARD)<<"first" ;
327 return target;
328 } else if (m_cur_t < 1.0 - m_step)
329 {
330 // calculate the power factors
331 target = p_compute_p5f(m_cur_t);
332 target += m_x0;
333
334 // time
335 m_cur_t += m_step;
336 m_cur_step++;
337
338 m_prev_a = target;
339 // yCDebug(GAZEBOCONTROLBOARD)<< target;
340 return target;
341 }
342
343 // yCDebug(GAZEBOCONTROLBOARD)<<"last";
345 target = m_xf;
346 return target;
347}
348
350{
351 std::lock_guard<std::mutex> lock(m_mutex);
352 double ret = p_computeTrajectory();
353 return ret;
354}
355
360
361//------------------------------------------------------------------------------------------------------------------
362// ConstSpeedTrajectoryGenerator
363//------------------------------------------------------------------------------------------------------------------
364
368
372
374 double final_pos,
375 double speed,
376 double acceleration,
377 std::chrono::milliseconds controller_period)
378{
379 std::lock_guard<std::mutex> lock(m_mutex);
380
381 m_controllerPeriodMilliseconds = controller_period.count();
382 m_x0 = current_pos;
383 m_xf = final_pos;
384 if (m_xf > m_joint_max)
386 else if (m_xf < m_joint_min)
388 m_speed = speed;
389 m_acceleration = acceleration; // unused
391 return true;
392}
393
394bool ConstSpeedTrajectoryGenerator::p_abortTrajectory(double limit)
395{
396 // to be implemented
397 return true;
398}
399
401{
402 std::lock_guard<std::mutex> lock(m_mutex);
403 bool ret = p_abortTrajectory(limit);
404 return ret;
405}
406
407double ConstSpeedTrajectoryGenerator::p_computeTrajectoryStep()
408{
409 double step = 0;
410 double error_abs = fabs(m_xf - m_computed_reference);
411
413 {
414 step = +(m_speed / 1000.0) * m_controllerPeriodMilliseconds; //* _T_controller;
415 } else
416 {
417 step = -(m_speed / 1000.0) * m_controllerPeriodMilliseconds; //* _T_controller;
418 }
419
420 if (error_abs < fabs(step))
421 {
422 step = (m_xf - m_computed_reference);
423 }
424
425 // yCDebug(GAZEBOCONTROLBOARD) << step;
426 return step;
427}
428
430{
431 std::lock_guard<std::mutex> lock(m_mutex);
432 double ret = p_computeTrajectoryStep();
433 return ret;
434}
435
437{
438 std::lock_guard<std::mutex> lock(m_mutex);
439 double ret = p_computeTrajectory();
440 return ret;
441}
442
443double ConstSpeedTrajectoryGenerator::p_computeTrajectory()
444{
445 double step = (m_speed / 1000.0) * m_controllerPeriodMilliseconds; //* _T_controller;
446 double error_abs = fabs(m_computed_reference - m_xf);
447
448 if (error_abs)
449 {
450 m_computed_reference += p_computeTrajectoryStep();
451 if (error_abs < step)
452 {
454 } else
455 {
456 m_trajectory_complete = false;
457 }
458 }
459
460 // yCDebug(GAZEBOCONTROLBOARD) << m_computed_reference;
462}
463
468
469//------------------------------------------------------------------------------------------------------------------
470// TrapezoidalSpeedTrajectoryGenerator
471//------------------------------------------------------------------------------------------------------------------
472
474 : m_computed_reference_velocity(0.0)
475{
476}
477
481
483 double final_pos,
484 double speed,
485 double acceleration,
486 std::chrono::milliseconds controller_period)
487{
488 std::lock_guard<std::mutex> lock(m_mutex);
489
490 if (speed <= 0.0 || acceleration <= 0.0)
491 {
492 return false;
493 }
494
495 m_x0 = current_pos;
496 m_xf = final_pos;
497 m_v0 = m_computed_reference_velocity; // last computed velocity from previous trajectory
498
499 if (m_xf > m_joint_max)
501 else if (m_xf < m_joint_min)
503
504 double sign = (m_xf >= m_x0) ? 1.0 : -1.0;
505 m_speed = sign * speed;
506 m_acceleration = sign * acceleration;
507
508 // hypothetic distance traveled using a double-ramp (i.e. triangular) trajectory that reaches
509 // m_speed
510 double deltaTriMax = 0.5 * (2 * m_speed * m_speed - m_v0 * m_v0) / m_acceleration;
511 double deltaTotal = m_xf - m_x0;
512
513 if (std::abs(deltaTotal) > std::abs(deltaTriMax))
514 {
515 // trapezoidal profile
516 double deltaRamp1 = 0.5 * (m_speed * m_speed - m_v0 * m_v0) / m_acceleration;
517 double deltaRamp2 = 0.5 * m_speed * m_speed / m_acceleration;
518 m_ta = (m_speed - m_v0) / m_acceleration;
519 m_tb = m_ta + (deltaTotal - deltaRamp1 - deltaRamp2) / m_speed;
520 m_tf = m_tb + (m_speed / m_acceleration);
521 } else
522 {
523 // triangular profile
524 double peakVelocity
525 = sign * std::sqrt(0.5 * (2 * m_acceleration * deltaTotal + m_v0 * m_v0));
526 m_ta = (peakVelocity - m_v0) / m_acceleration;
527 m_tb = m_ta;
528 m_tf = m_ta + (peakVelocity / m_acceleration);
529 }
530
531 m_computed_reference = current_pos;
532 m_controllerPeriodMilliseconds = controller_period.count();
533 m_tick = 0;
534 m_trajectory_complete = false;
535
536 return true;
537}
538
540{
541 std::lock_guard<std::mutex> lock(m_mutex);
543 m_computed_reference_velocity = 0.0;
544 return true;
545}
546
548{
549 std::lock_guard<std::mutex> lock(m_mutex);
550 m_computed_reference += p_computeTrajectoryStep();
552}
553
555{
556 std::lock_guard<std::mutex> lock(m_mutex);
557 return p_computeTrajectoryStep();
558}
559
560double TrapezoidalSpeedTrajectoryGenerator::p_computeTrajectoryStep()
561{
563 {
564 return 0;
565 }
566
567 double period = m_controllerPeriodMilliseconds / 1000.0; // in seconds
568 double instant = m_tick * period; // current time since start (in seconds)
569 double step = 0;
570
571 if (instant <= m_ta) // first ramp
572 {
573 step = 0.5 * m_acceleration * period * period * (2 * m_tick + 1) + m_v0 * period;
574 m_computed_reference_velocity = m_acceleration * instant + m_v0;
575 } else if (instant > m_tb) // second ramp
576 {
577 step = m_acceleration * (m_tf * period - 0.5 * period * period * (2 * m_tick + 1));
578 m_computed_reference_velocity = m_acceleration * (m_tf - instant);
579 } else // constant speed interval between ramps (if any)
580 {
581 step = m_speed * period;
582 m_computed_reference_velocity = m_speed;
583 }
584
585 if (std::abs(step) >= std::abs(m_xf - m_computed_reference) || instant > m_tf)
586 {
587 step = m_xf - m_computed_reference;
589 m_computed_reference_velocity = 0.0;
590 }
591
592 m_tick++;
593 return step;
594}
595
@ TRAJECTORY_TYPE_TRAP_SPEED
@ TRAJECTORY_TYPE_CONST_SPEED
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
TrajectoryType getTrajectoryType() override
bool abortTrajectory(double limit) override
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
TrajectoryType getTrajectoryType() override
bool abortTrajectory(double limit) override
void setReference(double ref, double step)
bool setLimits(double min, double max)
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
void modifyDuration(double expireTime)
Watchdog(double expireTime)
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121