sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
se3-trajectory-generator.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
9#include <sot/core/stop-watch.hh>
12
13namespace dynamicgraph {
14namespace sot {
15namespace torque_control {
16namespace dg = ::dynamicgraph;
17using namespace dg;
18using namespace dg::command;
19using namespace std;
20using namespace Eigen;
21
22#define PROFILE_SE3_POSITION_DESIRED_COMPUTATION "SE3TrajGen: traj computation"
23
26typedef SE3TrajectoryGenerator EntityClassName;
27
28/* --- DG FACTORY ---------------------------------------------------- */
30 "SE3TrajectoryGenerator");
31
32/* ------------------------------------------------------------------- */
33/* --- CONSTRUCTION -------------------------------------------------- */
34/* ------------------------------------------------------------------- */
36 : Entity(name),
37 CONSTRUCT_SIGNAL_IN(initial_value, dynamicgraph::Vector),
38 CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector),
39 CONSTRUCT_SIGNAL_IN(trigger, bool),
40 CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT),
41 CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT),
42 m_initSucceeded(false),
43 m_firstIter(true),
44 m_np(12),
45 m_nv(6),
46 m_iterLast(0),
47 m_t(0),
48 m_splineReady(false) {
49 BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
50
51 Entity::signalRegistration(m_xSOUT << m_dxSOUT << m_ddxSOUT
52 << m_initial_valueSIN << m_triggerSIN);
53
54 /* Commands. */
55 addCommand("init", makeCommandVoid1(
57 docCommandVoid1("Initialize the entity.",
58 "Time period in seconds (double)")));
59
60 addCommand(
61 "getValue",
62 makeCommandVoid1(
64 docCommandVoid1("Get the current value of the specified index.",
65 "index (int)")));
66
67 addCommand("playTrajectoryFile",
68 makeCommandVoid1(
70 docCommandVoid1(
71 "Play the trajectory read from the specified text file.",
72 "(string) File name, path included")));
73 addCommand(
74 "setSpline",
75 makeCommandVoid3(*this, &SE3TrajectoryGenerator::setSpline,
76 docCommandVoid3("Load serialized spline from file",
77 "(string) filename",
78 "(double) time to initial conf",
79 "(Matrix) orientation of the point")));
80
81 addCommand(
82 "startSinusoid",
83 makeCommandVoid3(
85 docCommandVoid3("Start an infinite sinusoid motion.",
86 "(int) index", "(double) final value",
87 "(double) time to reach the final value in sec")));
88
89 addCommand(
90 "startTriangle",
91 makeCommandVoid4(
93 docCommandVoid4("Start an infinite triangle wave.", "(int) index",
94 "(double) final values",
95 "(double) time to reach the final value in sec",
96 "(double) time to accelerate in sec")));
97
98 addCommand(
99 "startConstAcc",
100 makeCommandVoid3(
102 docCommandVoid3("Start an infinite trajectory with piece-wise "
103 "constant acceleration.",
104 "(int) index", "(double) final values",
105 "(double) time to reach the final value in sec")));
106
107 addCommand("startLinChirp",
108 makeCommandVoid5(
110 docCommandVoid5("Start a linear-chirp motion.",
111 "(int) index", "(double) final values",
112 "(double) initial frequency [Hz]",
113 "(double) final frequency [Hz]",
114 "(double) trajectory time [sec]")));
115 addCommand("move", makeCommandVoid3(
117 docCommandVoid3(
118 "Move component corresponding to index to the "
119 "specified value with a minimum jerk trajectory.",
120 "(int) index", "(double) final values",
121 "(double) time to reach the final value in sec")));
122 addCommand(
123 "stop",
124 makeCommandVoid1(
126 docCommandVoid1("Stop the motion of the specified index, or of all "
127 "components of the vector if index is equal to -1.",
128 "(int) index")));
129}
130
131void SE3TrajectoryGenerator::init(const double& dt) {
132 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
133 m_firstIter = true;
134 m_dt = dt;
135 m_status.resize(m_np, TG_STOP);
136 m_minJerkTrajGen.resize(m_np);
137 m_sinTrajGen.resize(m_np);
138 m_triangleTrajGen.resize(m_np);
139 m_constAccTrajGen.resize(m_np);
140 m_linChirpTrajGen.resize(m_np);
141 m_currentTrajGen.resize(m_np);
142 m_noTrajGen.resize(m_np);
143 for (unsigned int i = 0; i < m_np; i++) {
145 m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 1);
152 }
153 m_splineTrajGen = new parametriccurves::Spline<double, Eigen::Dynamic>();
155 m_initSucceeded = true;
156}
157
158/* ------------------------------------------------------------------- */
159/* --- SIGNALS ------------------------------------------------------- */
160/* ------------------------------------------------------------------- */
161
162DEFINE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector) {
163 if (!m_initSucceeded) {
164 SEND_WARNING_STREAM_MSG(
165 "Cannot compute signal positionDes before initialization!");
166 return s;
167 }
168
169 getProfiler().start(PROFILE_SE3_POSITION_DESIRED_COMPUTATION);
170 {
171 if (s.size() != m_np) s.resize(m_np);
172
173 // at first iteration store initial values
174 if (m_firstIter) {
175 const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
176 if (initial_value.size() != m_np) {
177 SEND_ERROR_STREAM_MSG(
178 "Unexpected size of input signal initial_value: " +
179 toString(initial_value.size()));
181 return s;
182 }
183 for (unsigned int i = 0; i < m_np; i++)
184 m_currentTrajGen[i]->set_initial_point(initial_value(i));
185 m_firstIter = false;
186 } else if (iter == static_cast<int>(m_iterLast)) {
187 if (m_triggerSIN(iter) == true && m_splineReady) startSpline();
188 if (m_status[0] == TG_TEXT_FILE) {
189 for (unsigned int i = 0; i < m_np; i++)
190 s(i) = m_textFileTrajGen->getPos()[i];
191 } else if (m_status[0] == TG_SPLINE) {
192 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
193 s.head<3>() = t;
194 s.segment<3>(3) = m_splineRotation.row(0);
195 s.segment<3>(6) = m_splineRotation.row(1);
196 s.segment<3>(9) = m_splineRotation.row(2);
197 } else
198 for (unsigned int i = 0; i < m_np; i++)
199 s(i) = m_currentTrajGen[i]->getPos()(0);
201 return s;
202 }
203 m_iterLast = iter;
204 if (m_triggerSIN(iter) == true && m_splineReady) startSpline();
205 if (m_status[0] == TG_TEXT_FILE) {
206 const VectorXd& xRef = m_textFileTrajGen->compute_next_point();
207 for (unsigned int i = 0; i < m_np; i++) {
208 s(i) = xRef[i];
209 if (m_textFileTrajGen->isTrajectoryEnded()) {
210 m_noTrajGen[i]->set_initial_point(s(i));
211 m_status[i] = TG_STOP;
212 }
213 }
214 if (m_textFileTrajGen->isTrajectoryEnded())
215 SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
216 } else if (m_status[0] == TG_SPLINE) {
217 m_t += m_dt;
218 if (!m_splineTrajGen->checkRange(m_t)) {
219 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_splineTrajGen->tmax());
220 s.head<3>() = t;
221 s.segment<3>(3) = m_splineRotation.row(0);
222 s.segment<3>(6) = m_splineRotation.row(1);
223 s.segment<3>(9) = m_splineRotation.row(2);
224 for (unsigned int i = 0; i < m_np; i++) {
225 m_noTrajGen[i]->set_initial_point(s(i));
226 m_status[i] = TG_STOP;
227 }
228 m_splineReady = false;
229 SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.",
230 MSG_TYPE_INFO);
231 m_t = 0;
232 } else {
233 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
234 s.head<3>() = t;
235 s.segment<3>(3) = m_splineRotation.row(0);
236 s.segment<3>(6) = m_splineRotation.row(1);
237 s.segment<3>(9) = m_splineRotation.row(2);
238 }
239 } else {
240 for (unsigned int i = 0; i < m_np; i++) {
241 s(i) = m_currentTrajGen[i]->compute_next_point()(0);
242 if (m_currentTrajGen[i]->isTrajectoryEnded()) {
243 m_currentTrajGen[i] = m_noTrajGen[i];
244 m_noTrajGen[i]->set_initial_point(s(i));
245 m_status[i] = TG_STOP;
246 SEND_MSG("Trajectory of index " + toString(i) + " ended.",
247 MSG_TYPE_INFO);
248 }
249 }
250 }
251 }
253
254 return s;
255}
256
257DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
258 if (!m_initSucceeded) {
259 std::ostringstream oss(
260 "Cannot compute signal positionDes before initialization! iter:");
261 oss << iter;
262 SEND_WARNING_STREAM_MSG(oss.str());
263 return s;
264 }
265
266 if (s.size() != m_nv) s.resize(m_nv);
267 if (m_status[0] == TG_TEXT_FILE) {
268 for (unsigned int i = 0; i < m_nv; i++)
269 s(i) = m_textFileTrajGen->getVel()[i];
270 } else if (m_status[0] == TG_SPLINE) {
271 const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 1);
272 s.segment<3>(0) = t;
273 s.segment<3>(3).setZero();
274 } else
275 for (unsigned int i = 0; i < m_nv; i++)
276 s(i) = m_currentTrajGen[i]->getVel()(0);
277
278 return s;
279}
280
281DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
282 if (!m_initSucceeded) {
283 std::ostringstream oss(
284 "Cannot compute signal positionDes before initialization! iter:");
285 oss << iter;
286 SEND_WARNING_STREAM_MSG(oss.str());
287 return s;
288 }
289
290 if (s.size() != m_nv) s.resize(m_nv);
291
292 if (m_status[0] == TG_TEXT_FILE) {
293 for (unsigned int i = 0; i < m_nv; i++)
294 s(i) = m_textFileTrajGen->getAcc()[i];
295 } else if (m_status[0] == TG_SPLINE) {
296 const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 2);
297 s.segment<3>(0) = t;
298 s.segment<3>(3).setZero();
299 } else
300 for (unsigned int i = 0; i < m_nv; i++)
301 s(i) = m_currentTrajGen[i]->getAcc()(0);
302
303 return s;
304}
305
306/* ------------------------------------------------------------------- */
307/* --- COMMANDS ------------------------------------------------------ */
308/* ------------------------------------------------------------------- */
309
311 if (id < 0 || id >= static_cast<int>(m_np))
312 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
313
314 SEND_MSG("Current value of component " + toString(id) + " is " +
315 toString(m_currentTrajGen[id]->getPos()(0)),
316 MSG_TYPE_INFO);
317}
318
319void SE3TrajectoryGenerator::playTrajectoryFile(const std::string& fileName) {
320 if (!m_initSucceeded)
321 return SEND_MSG("Cannot start sinusoid before initialization!",
322 MSG_TYPE_ERROR);
323
324 for (unsigned int i = 0; i < m_np; i++)
325 if (m_status[i] != TG_STOP)
326 return SEND_MSG("You cannot control component " + toString(i) +
327 " because it is already controlled.",
328 MSG_TYPE_ERROR);
329
330 if (!m_textFileTrajGen->loadTextFile(fileName))
331 return SEND_MSG("Error while loading text file " + fileName,
332 MSG_TYPE_ERROR);
333
334 // check current configuration is not too far from initial trajectory
335 // configuration
336 bool needToMoveToInitConf = false;
337 const VectorXd& xInit = m_textFileTrajGen->get_initial_point();
338 for (unsigned int i = 0; i < m_np; i++)
339 if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
340 needToMoveToInitConf = true;
341 SEND_MSG("Component " + toString(i) +
342 " is too far from initial configuration so first i will "
343 "move it there.",
344 MSG_TYPE_WARNING);
345 }
346
347 // if necessary move joints to initial configuration
348 if (needToMoveToInitConf) {
349 for (unsigned int i = 0; i < m_np; i++) {
350 m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
351 m_minJerkTrajGen[i]->set_final_point(xInit[i]);
352 m_minJerkTrajGen[i]->set_trajectory_time(4.0);
355 }
356 return;
357 }
358
359 for (unsigned int i = 0; i < m_np; i++) {
361 }
362}
363
364void SE3TrajectoryGenerator::setSpline(const std::string& fileName,
365 const double& timeToInitConf,
366 const Eigen::MatrixXd& init_rotation) {
367 if (!m_initSucceeded)
368 return SEND_MSG("Cannot start sinusoid before initialization!",
369 MSG_TYPE_ERROR);
370
371 for (unsigned int i = 0; i < m_np; i++)
372 if (m_status[i] != TG_STOP)
373 return SEND_MSG("You cannot control component " + toString(i) +
374 " because it is already controlled.",
375 MSG_TYPE_ERROR);
376
377 if (!m_splineTrajGen->loadFromFile(fileName))
378 return SEND_MSG("Error while loading text file " + fileName,
379 MSG_TYPE_ERROR);
380
381 // check current configuration is not too far from initial trajectory
382 // configuration
383 bool needToMoveToInitConf = false;
384 m_splineReady = true;
385 m_splineRotation = init_rotation;
386 if (timeToInitConf < 0) {
387 SEND_MSG("Spline Ready. Set trigger to true to start playing",
388 MSG_TYPE_INFO);
389 return;
390 }
391
392 const VectorXd& t = (*m_splineTrajGen)(0.0);
393 VectorXd xInit(12);
394 xInit.segment<3>(3) = m_splineRotation.row(0);
395 xInit.segment<3>(6) = m_splineRotation.row(1);
396 xInit.segment<3>(9) = m_splineRotation.row(2);
397 xInit.head<3>() = t;
398
399 for (unsigned int i = 0; i < m_np; i++)
400 if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
401 needToMoveToInitConf = true;
402 SEND_MSG("Component " + toString(i) +
403 " is too far from initial configuration so first i will "
404 "move it there.",
405 MSG_TYPE_WARNING);
406 }
407
408 // if necessary move joints to initial configuration
409 if (needToMoveToInitConf) {
410 for (unsigned int i = 0; i < m_np; i++) {
411 m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
412 m_minJerkTrajGen[i]->set_final_point(xInit[i]);
413 m_minJerkTrajGen[i]->set_trajectory_time(timeToInitConf);
416 }
417 return;
418 }
419
420 SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
421}
422
424 if (m_status[0] == TG_SPLINE) return;
425 m_t = 0.0;
426 for (unsigned int i = 0; i < m_np; i++) {
427 m_status[i] = TG_SPLINE;
428 }
429}
430
431void SE3TrajectoryGenerator::startSinusoid(const int& id, const double& xFinal,
432 const double& time) {
433 if (!m_initSucceeded)
434 return SEND_MSG("Cannot start sinusoid before initialization!",
435 MSG_TYPE_ERROR);
436
437 if (id < 0 || id >= static_cast<int>(m_np))
438 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
439 unsigned int i = id;
440 if (time <= 0.0)
441 return SEND_MSG("Trajectory time must be a positive number",
442 MSG_TYPE_ERROR);
443 if (m_status[i] != TG_STOP)
444 return SEND_MSG(
445 "You cannot move the specified component because it is already "
446 "controlled.",
447 MSG_TYPE_ERROR);
448
449 m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
450 SEND_MSG(
451 "Set initial point of sinusoid to " + toString(m_sinTrajGen[i]->getPos()),
452 MSG_TYPE_DEBUG);
453 m_sinTrajGen[i]->set_final_point(xFinal);
454 m_sinTrajGen[i]->set_trajectory_time(time);
457}
458
459void SE3TrajectoryGenerator::startTriangle(const int& id, const double& xFinal,
460 const double& time,
461 const double& Tacc) {
462 if (!m_initSucceeded)
463 return SEND_MSG("Cannot start triangle before initialization!",
464 MSG_TYPE_ERROR);
465 if (id < 0 || id >= static_cast<int>(m_np))
466 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
467 unsigned int i = id;
468 if (m_status[i] != TG_STOP)
469 return SEND_MSG(
470 "You cannot move the specified component because it is already "
471 "controlled.",
472 MSG_TYPE_ERROR);
473
474 m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
475 SEND_MSG("Set initial point of triangular trajectory to " +
476 toString(m_triangleTrajGen[i]->getPos()),
477 MSG_TYPE_DEBUG);
478 m_triangleTrajGen[i]->set_final_point(xFinal);
479
480 if (!m_triangleTrajGen[i]->set_trajectory_time(time))
481 return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
482
483 if (!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
484 return SEND_MSG(
485 "Acceleration time cannot be negative or larger than half the "
486 "trajectory time.",
487 MSG_TYPE_ERROR);
488
491}
492
493void SE3TrajectoryGenerator::startConstAcc(const int& id, const double& xFinal,
494 const double& time) {
495 if (!m_initSucceeded)
496 return SEND_MSG(
497 "Cannot start constant-acceleration trajectory before initialization!",
498 MSG_TYPE_ERROR);
499 if (id < 0 || id >= static_cast<int>(m_np))
500 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
501 unsigned int i = id;
502 if (time <= 0.0)
503 return SEND_MSG("Trajectory time must be a positive number",
504 MSG_TYPE_ERROR);
505 if (m_status[i] != TG_STOP)
506 return SEND_MSG(
507 "You cannot move the specified component because it is already "
508 "controlled.",
509 MSG_TYPE_ERROR);
510
511 m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
512 SEND_MSG("Set initial point of const-acc trajectory to " +
513 toString(m_constAccTrajGen[i]->getPos()),
514 MSG_TYPE_DEBUG);
515 m_constAccTrajGen[i]->set_final_point(xFinal);
516 m_constAccTrajGen[i]->set_trajectory_time(time);
519}
520
522 const double& xFinal,
523 const double& f0,
524 const double& f1,
525 const double& time) {
526 if (!m_initSucceeded)
527 return SEND_MSG("Cannot start linear chirp before initialization!",
528 MSG_TYPE_ERROR);
529 if (id < 0 || id >= static_cast<int>(m_np))
530 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
531 unsigned int i = id;
532 if (time <= 0.0)
533 return SEND_MSG("Trajectory time must be a positive number",
534 MSG_TYPE_ERROR);
535 if (m_status[i] != TG_STOP)
536 return SEND_MSG(
537 "You cannot move the specified component because it is already "
538 "controlled.",
539 MSG_TYPE_ERROR);
540 if (f0 > f1)
541 return SEND_MSG(
542 "f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1),
543 MSG_TYPE_ERROR);
544 if (f0 <= 0.0)
545 return SEND_MSG("Frequency has to be positive " + toString(f0),
546 MSG_TYPE_ERROR);
547
548 if (!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()))
549 return SEND_MSG("Error while setting initial point " +
550 toString(m_noTrajGen[i]->getPos()),
551 MSG_TYPE_ERROR);
552 if (!m_linChirpTrajGen[i]->set_final_point(xFinal))
553 return SEND_MSG("Error while setting final point " + toString(xFinal),
554 MSG_TYPE_ERROR);
555 if (!m_linChirpTrajGen[i]->set_trajectory_time(time))
556 return SEND_MSG("Error while setting trajectory time " + toString(time),
557 MSG_TYPE_ERROR);
558 if (!m_linChirpTrajGen[i]->set_initial_frequency(f0))
559 return SEND_MSG("Error while setting initial frequency " + toString(f0),
560 MSG_TYPE_ERROR);
561 if (!m_linChirpTrajGen[i]->set_final_frequency(f1))
562 return SEND_MSG("Error while setting final frequency " + toString(f1),
563 MSG_TYPE_ERROR);
566}
567
568void SE3TrajectoryGenerator::move(const int& id, const double& xFinal,
569 const double& time) {
570 if (!m_initSucceeded)
571 return SEND_MSG("Cannot move value before initialization!", MSG_TYPE_ERROR);
572 unsigned int i = id;
573 if (id < 0 || id >= static_cast<int>(m_np))
574 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
575 if (time <= 0.0)
576 return SEND_MSG("Trajectory time must be a positive number",
577 MSG_TYPE_ERROR);
578 if (m_status[i] != TG_STOP)
579 return SEND_MSG(
580 "You cannot move the specified component because it is already "
581 "controlled.",
582 MSG_TYPE_ERROR);
583
584 m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
585 m_minJerkTrajGen[i]->set_final_point(xFinal);
586 m_minJerkTrajGen[i]->set_trajectory_time(time);
589}
590
591void SE3TrajectoryGenerator::stop(const int& id) {
592 if (!m_initSucceeded)
593 return SEND_MSG("Cannot stop value before initialization!", MSG_TYPE_ERROR);
594
595 if (id == -1) // Stop entire vector
596 {
597 for (unsigned int i = 0; i < m_np; i++) {
598 m_status[i] = TG_STOP;
599 // update the initial value
600 m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos());
602 }
603 return;
604 }
605 if (id < 0 || id >= static_cast<int>(m_np))
606 return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
607 unsigned int i = id;
608 m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos());
609 m_status[i] = TG_STOP;
611
612 m_splineReady = false;
613 m_t = 0.0;
614}
615
616/* ------------------------------------------------------------------- */
617/* --- ENTITY -------------------------------------------------------- */
618/* ------------------------------------------------------------------- */
619
620void SE3TrajectoryGenerator::display(std::ostream& os) const {
621 os << "SE3TrajectoryGenerator " << getName();
622 try {
623 getProfiler().report_all(3, os);
624 } catch (ExceptionSignal e) {
625 }
626}
627} // namespace torque_control
628} // namespace sot
629} // namespace dynamicgraph
void move(const int &id, const double &xFinal, const double &time)
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
void setSpline(const std::string &filename, const double &timeToInitConf, const Eigen::MatrixXd &init_rotation)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
double m_dt
true if it is the first iteration, false otherwise
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
void startTriangle(const int &id, const double &xFinal, const double &time, const double &Tacc)
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
void startLinearChirp(const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
void startConstAcc(const int &id, const double &xFinal, const double &time)
void startSinusoid(const int &id, const double &xFinal, const double &time)
bool m_firstIter
true if the entity has been successfully initialized
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the component
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22
#define PROFILE_SE3_POSITION_DESIRED_COMPUTATION