sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
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 #include <sot/core/debug.hh>
8 
11 #include <sot/core/stop-watch.hh>
12 
13 namespace dynamicgraph {
14 namespace sot {
15 namespace torque_control {
16 namespace dg = ::dynamicgraph;
17 using namespace dg;
18 using namespace dg::command;
19 using namespace std;
20 using namespace Eigen;
21 
22 #define PROFILE_SE3_POSITION_DESIRED_COMPUTATION "SE3TrajGen: traj computation"
23 
26 typedef SE3TrajectoryGenerator EntityClassName;
27 
28 /* --- DG FACTORY ---------------------------------------------------- */
29 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SE3TrajectoryGenerator, "SE3TrajectoryGenerator");
30 
31 /* ------------------------------------------------------------------- */
32 /* --- CONSTRUCTION -------------------------------------------------- */
33 /* ------------------------------------------------------------------- */
35  : Entity(name),
36  CONSTRUCT_SIGNAL_IN(initial_value, dynamicgraph::Vector),
37  CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector),
38  CONSTRUCT_SIGNAL_IN(trigger, bool),
39  CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT),
40  CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT),
41  m_initSucceeded(false),
42  m_firstIter(true),
43  m_np(12),
44  m_nv(6),
45  m_iterLast(0),
46  m_t(0),
47  m_splineReady(false) {
48  BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
49 
50  Entity::signalRegistration(m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN << m_triggerSIN);
51 
52  /* Commands. */
53  addCommand("init", makeCommandVoid1(*this, &SE3TrajectoryGenerator::init,
54  docCommandVoid1("Initialize the entity.", "Time period in seconds (double)")));
55 
56  addCommand("getValue",
57  makeCommandVoid1(*this, &SE3TrajectoryGenerator::getValue,
58  docCommandVoid1("Get the current value of the specified index.", "index (int)")));
59 
60  addCommand("playTrajectoryFile",
61  makeCommandVoid1(*this, &SE3TrajectoryGenerator::playTrajectoryFile,
62  docCommandVoid1("Play the trajectory read from the specified text file.",
63  "(string) File name, path included")));
64  addCommand("setSpline",
65  makeCommandVoid3(*this, &SE3TrajectoryGenerator::setSpline,
66  docCommandVoid3("Load serialized spline from file", "(string) filename",
67  "(double) time to initial conf", "(Matrix) orientation of the point")));
68 
69  addCommand("startSinusoid", makeCommandVoid3(*this, &SE3TrajectoryGenerator::startSinusoid,
70  docCommandVoid3("Start an infinite sinusoid motion.", "(int) index",
71  "(double) final value",
72  "(double) time to reach the final value in sec")));
73 
74  addCommand("startTriangle",
75  makeCommandVoid4(*this, &SE3TrajectoryGenerator::startTriangle,
76  docCommandVoid4("Start an infinite triangle wave.", "(int) index",
77  "(double) final values", "(double) time to reach the final value in sec",
78  "(double) time to accelerate in sec")));
79 
80  addCommand("startConstAcc",
81  makeCommandVoid3(*this, &SE3TrajectoryGenerator::startConstAcc,
82  docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
83  "(int) index", "(double) final values",
84  "(double) time to reach the final value in sec")));
85 
86  addCommand("startLinChirp",
88  docCommandVoid5("Start a linear-chirp motion.", "(int) index",
89  "(double) final values", "(double) initial frequency [Hz]",
90  "(double) final frequency [Hz]", "(double) trajectory time [sec]")));
91  addCommand("move",
92  makeCommandVoid3(
94  docCommandVoid3(
95  "Move component corresponding to index to the specified value with a minimum jerk trajectory.",
96  "(int) index", "(double) final values", "(double) time to reach the final value in sec")));
97  addCommand(
98  "stop",
99  makeCommandVoid1(
101  docCommandVoid1(
102  "Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
103  "(int) index")));
104 }
105 
106 void SE3TrajectoryGenerator::init(const double& dt) {
107  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
108  m_firstIter = true;
109  m_dt = dt;
110  m_status.resize(m_np, TG_STOP);
111  m_minJerkTrajGen.resize(m_np);
112  m_sinTrajGen.resize(m_np);
113  m_triangleTrajGen.resize(m_np);
114  m_constAccTrajGen.resize(m_np);
115  m_linChirpTrajGen.resize(m_np);
116  m_currentTrajGen.resize(m_np);
117  m_noTrajGen.resize(m_np);
118  for (unsigned int i = 0; i < m_np; i++) {
120  m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 1);
121  m_triangleTrajGen[i] = new TriangleTrajectoryGenerator(dt, 5.0, 1);
126  }
127  m_splineTrajGen = new parametriccurves::Spline<double, Eigen::Dynamic>();
129  m_initSucceeded = true;
130 }
131 
132 /* ------------------------------------------------------------------- */
133 /* --- SIGNALS ------------------------------------------------------- */
134 /* ------------------------------------------------------------------- */
135 
136 DEFINE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector) {
137  if (!m_initSucceeded) {
138  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
139  return s;
140  }
141 
142  getProfiler().start(PROFILE_SE3_POSITION_DESIRED_COMPUTATION);
143  {
144  if (s.size() != m_np) s.resize(m_np);
145 
146  // at first iteration store initial values
147  if (m_firstIter) {
148  const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
149  if (initial_value.size() != m_np) {
150  SEND_ERROR_STREAM_MSG("Unexpected size of input signal initial_value: " + toString(initial_value.size()));
151  getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION);
152  return s;
153  }
154  for (unsigned int i = 0; i < m_np; i++) m_currentTrajGen[i]->set_initial_point(initial_value(i));
155  m_firstIter = false;
156  } else if (iter == static_cast<int>(m_iterLast)) {
157  if (m_triggerSIN(iter) == true && m_splineReady) startSpline();
158  if (m_status[0] == TG_TEXT_FILE) {
159  for (unsigned int i = 0; i < m_np; i++) s(i) = m_textFileTrajGen->getPos()[i];
160  } else if (m_status[0] == TG_SPLINE) {
161  const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
162  s.head<3>() = t;
163  s.segment<3>(3) = m_splineRotation.row(0);
164  s.segment<3>(6) = m_splineRotation.row(1);
165  s.segment<3>(9) = m_splineRotation.row(2);
166  } else
167  for (unsigned int i = 0; i < m_np; i++) s(i) = m_currentTrajGen[i]->getPos()(0);
168  getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION);
169  return s;
170  }
171  m_iterLast = iter;
172  if (m_triggerSIN(iter) == true && m_splineReady) startSpline();
173  if (m_status[0] == TG_TEXT_FILE) {
174  const VectorXd& xRef = m_textFileTrajGen->compute_next_point();
175  for (unsigned int i = 0; i < m_np; i++) {
176  s(i) = xRef[i];
177  if (m_textFileTrajGen->isTrajectoryEnded()) {
178  m_noTrajGen[i]->set_initial_point(s(i));
179  m_status[i] = TG_STOP;
180  }
181  }
182  if (m_textFileTrajGen->isTrajectoryEnded()) SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
183  } else if (m_status[0] == TG_SPLINE) {
184  m_t += m_dt;
185  if (!m_splineTrajGen->checkRange(m_t)) {
186  const Eigen::Vector3d& t = (*m_splineTrajGen)(m_splineTrajGen->tmax());
187  s.head<3>() = t;
188  s.segment<3>(3) = m_splineRotation.row(0);
189  s.segment<3>(6) = m_splineRotation.row(1);
190  s.segment<3>(9) = m_splineRotation.row(2);
191  for (unsigned int i = 0; i < m_np; i++) {
192  m_noTrajGen[i]->set_initial_point(s(i));
193  m_status[i] = TG_STOP;
194  }
195  m_splineReady = false;
196  SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
197  m_t = 0;
198  } else {
199  const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
200  s.head<3>() = t;
201  s.segment<3>(3) = m_splineRotation.row(0);
202  s.segment<3>(6) = m_splineRotation.row(1);
203  s.segment<3>(9) = m_splineRotation.row(2);
204  }
205  } else {
206  for (unsigned int i = 0; i < m_np; i++) {
207  s(i) = m_currentTrajGen[i]->compute_next_point()(0);
208  if (m_currentTrajGen[i]->isTrajectoryEnded()) {
209  m_currentTrajGen[i] = m_noTrajGen[i];
210  m_noTrajGen[i]->set_initial_point(s(i));
211  m_status[i] = TG_STOP;
212  SEND_MSG("Trajectory of index " + toString(i) + " ended.", MSG_TYPE_INFO);
213  }
214  }
215  }
216  }
217  getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION);
218 
219  return s;
220 }
221 
222 DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
223  if (!m_initSucceeded) {
224  std::ostringstream oss("Cannot compute signal positionDes before initialization! iter:");
225  oss << iter;
226  SEND_WARNING_STREAM_MSG(oss.str());
227  return s;
228  }
229 
230  if (s.size() != m_nv) s.resize(m_nv);
231  if (m_status[0] == TG_TEXT_FILE) {
232  for (unsigned int i = 0; i < m_nv; i++) s(i) = m_textFileTrajGen->getVel()[i];
233  } else if (m_status[0] == TG_SPLINE) {
234  const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 1);
235  s.segment<3>(0) = t;
236  s.segment<3>(3).setZero();
237  } else
238  for (unsigned int i = 0; i < m_nv; i++) s(i) = m_currentTrajGen[i]->getVel()(0);
239 
240  return s;
241 }
242 
243 DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
244  if (!m_initSucceeded) {
245  std::ostringstream oss("Cannot compute signal positionDes before initialization! iter:");
246  oss << iter;
247  SEND_WARNING_STREAM_MSG(oss.str());
248  return s;
249  }
250 
251  if (s.size() != m_nv) s.resize(m_nv);
252 
253  if (m_status[0] == TG_TEXT_FILE) {
254  for (unsigned int i = 0; i < m_nv; i++) s(i) = m_textFileTrajGen->getAcc()[i];
255  } else if (m_status[0] == TG_SPLINE) {
256  const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 2);
257  s.segment<3>(0) = t;
258  s.segment<3>(3).setZero();
259  } else
260  for (unsigned int i = 0; i < m_nv; i++) s(i) = m_currentTrajGen[i]->getAcc()(0);
261 
262  return s;
263 }
264 
265 /* ------------------------------------------------------------------- */
266 /* --- COMMANDS ------------------------------------------------------ */
267 /* ------------------------------------------------------------------- */
268 
269 void SE3TrajectoryGenerator::getValue(const int& id) {
270  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
271 
272  SEND_MSG("Current value of component " + toString(id) + " is " + toString(m_currentTrajGen[id]->getPos()(0)),
273  MSG_TYPE_INFO);
274 }
275 
276 void SE3TrajectoryGenerator::playTrajectoryFile(const std::string& fileName) {
277  if (!m_initSucceeded) return SEND_MSG("Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
278 
279  for (unsigned int i = 0; i < m_np; i++)
280  if (m_status[i] != TG_STOP)
281  return SEND_MSG("You cannot control component " + toString(i) + " because it is already controlled.",
282  MSG_TYPE_ERROR);
283 
284  if (!m_textFileTrajGen->loadTextFile(fileName))
285  return SEND_MSG("Error while loading text file " + fileName, MSG_TYPE_ERROR);
286 
287  // check current configuration is not too far from initial trajectory configuration
288  bool needToMoveToInitConf = false;
289  const VectorXd& xInit = m_textFileTrajGen->get_initial_point();
290  for (unsigned int i = 0; i < m_np; i++)
291  if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
292  needToMoveToInitConf = true;
293  SEND_MSG("Component " + toString(i) + " is too far from initial configuration so first i will move it there.",
294  MSG_TYPE_WARNING);
295  }
296 
297  // if necessary move joints to initial configuration
298  if (needToMoveToInitConf) {
299  for (unsigned int i = 0; i < m_np; i++) {
300  m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
301  m_minJerkTrajGen[i]->set_final_point(xInit[i]);
302  m_minJerkTrajGen[i]->set_trajectory_time(4.0);
303  m_status[i] = TG_MIN_JERK;
305  }
306  return;
307  }
308 
309  for (unsigned int i = 0; i < m_np; i++) {
310  m_status[i] = TG_TEXT_FILE;
311  }
312 }
313 
314 void SE3TrajectoryGenerator::setSpline(const std::string& fileName, const double& timeToInitConf,
315  const Eigen::MatrixXd& init_rotation) {
316  if (!m_initSucceeded) return SEND_MSG("Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
317 
318  for (unsigned int i = 0; i < m_np; i++)
319  if (m_status[i] != TG_STOP)
320  return SEND_MSG("You cannot control component " + toString(i) + " because it is already controlled.",
321  MSG_TYPE_ERROR);
322 
323  if (!m_splineTrajGen->loadFromFile(fileName))
324  return SEND_MSG("Error while loading text file " + fileName, MSG_TYPE_ERROR);
325 
326  // check current configuration is not too far from initial trajectory configuration
327  bool needToMoveToInitConf = false;
328  m_splineReady = true;
329  m_splineRotation = init_rotation;
330  if (timeToInitConf < 0) {
331  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
332  return;
333  }
334 
335  const VectorXd& t = (*m_splineTrajGen)(0.0);
336  VectorXd xInit(12);
337  xInit.segment<3>(3) = m_splineRotation.row(0);
338  xInit.segment<3>(6) = m_splineRotation.row(1);
339  xInit.segment<3>(9) = m_splineRotation.row(2);
340  xInit.head<3>() = t;
341 
342  for (unsigned int i = 0; i < m_np; i++)
343  if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
344  needToMoveToInitConf = true;
345  SEND_MSG("Component " + toString(i) + " is too far from initial configuration so first i will move it there.",
346  MSG_TYPE_WARNING);
347  }
348 
349  // if necessary move joints to initial configuration
350  if (needToMoveToInitConf) {
351  for (unsigned int i = 0; i < m_np; i++) {
352  m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
353  m_minJerkTrajGen[i]->set_final_point(xInit[i]);
354  m_minJerkTrajGen[i]->set_trajectory_time(timeToInitConf);
355  m_status[i] = TG_MIN_JERK;
357  }
358  return;
359  }
360 
361  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
362 }
363 
365  if (m_status[0] == TG_SPLINE) return;
366  m_t = 0.0;
367  for (unsigned int i = 0; i < m_np; i++) {
368  m_status[i] = TG_SPLINE;
369  }
370 }
371 
372 void SE3TrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time) {
373  if (!m_initSucceeded) return SEND_MSG("Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
374 
375  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
376  unsigned int i = id;
377  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
378  if (m_status[i] != TG_STOP)
379  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
380 
381  m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
382  SEND_MSG("Set initial point of sinusoid to " + toString(m_sinTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
383  m_sinTrajGen[i]->set_final_point(xFinal);
384  m_sinTrajGen[i]->set_trajectory_time(time);
385  m_status[i] = TG_SINUSOID;
387 }
388 
389 void SE3TrajectoryGenerator::startTriangle(const int& id, const double& xFinal, const double& time,
390  const double& Tacc) {
391  if (!m_initSucceeded) return SEND_MSG("Cannot start triangle before initialization!", MSG_TYPE_ERROR);
392  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
393  unsigned int i = id;
394  if (m_status[i] != TG_STOP)
395  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
396 
397  m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
398  SEND_MSG("Set initial point of triangular trajectory to " + toString(m_triangleTrajGen[i]->getPos()),
399  MSG_TYPE_DEBUG);
400  m_triangleTrajGen[i]->set_final_point(xFinal);
401 
402  if (!m_triangleTrajGen[i]->set_trajectory_time(time))
403  return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
404 
405  if (!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
406  return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
407 
408  m_status[i] = TG_TRIANGLE;
410 }
411 
412 void SE3TrajectoryGenerator::startConstAcc(const int& id, const double& xFinal, const double& time) {
413  if (!m_initSucceeded)
414  return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!", MSG_TYPE_ERROR);
415  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
416  unsigned int i = id;
417  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
418  if (m_status[i] != TG_STOP)
419  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
420 
421  m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
422  SEND_MSG("Set initial point of const-acc trajectory to " + toString(m_constAccTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
423  m_constAccTrajGen[i]->set_final_point(xFinal);
424  m_constAccTrajGen[i]->set_trajectory_time(time);
425  m_status[i] = TG_CONST_ACC;
427 }
428 
429 void SE3TrajectoryGenerator::startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1,
430  const double& time) {
431  if (!m_initSucceeded) return SEND_MSG("Cannot start linear chirp before initialization!", MSG_TYPE_ERROR);
432  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
433  unsigned int i = id;
434  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
435  if (m_status[i] != TG_STOP)
436  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
437  if (f0 > f1) return SEND_MSG("f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
438  if (f0 <= 0.0) return SEND_MSG("Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
439 
440  if (!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()))
441  return SEND_MSG("Error while setting initial point " + toString(m_noTrajGen[i]->getPos()), MSG_TYPE_ERROR);
442  if (!m_linChirpTrajGen[i]->set_final_point(xFinal))
443  return SEND_MSG("Error while setting final point " + toString(xFinal), MSG_TYPE_ERROR);
444  if (!m_linChirpTrajGen[i]->set_trajectory_time(time))
445  return SEND_MSG("Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
446  if (!m_linChirpTrajGen[i]->set_initial_frequency(f0))
447  return SEND_MSG("Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
448  if (!m_linChirpTrajGen[i]->set_final_frequency(f1))
449  return SEND_MSG("Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
450  m_status[i] = TG_LIN_CHIRP;
452 }
453 
454 void SE3TrajectoryGenerator::move(const int& id, const double& xFinal, const double& time) {
455  if (!m_initSucceeded) return SEND_MSG("Cannot move value before initialization!", MSG_TYPE_ERROR);
456  unsigned int i = id;
457  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
458  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
459  if (m_status[i] != TG_STOP)
460  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
461 
462  m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
463  m_minJerkTrajGen[i]->set_final_point(xFinal);
464  m_minJerkTrajGen[i]->set_trajectory_time(time);
465  m_status[i] = TG_MIN_JERK;
467 }
468 
469 void SE3TrajectoryGenerator::stop(const int& id) {
470  if (!m_initSucceeded) return SEND_MSG("Cannot stop value before initialization!", MSG_TYPE_ERROR);
471 
472  if (id == -1) // Stop entire vector
473  {
474  for (unsigned int i = 0; i < m_np; i++) {
475  m_status[i] = TG_STOP;
476  // update the initial value
477  m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos());
479  }
480  return;
481  }
482  if (id < 0 || id >= static_cast<int>(m_np)) return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
483  unsigned int i = id;
484  m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos());
485  m_status[i] = TG_STOP;
487 
488  m_splineReady = false;
489  m_t = 0.0;
490 }
491 
492 /* ------------------------------------------------------------------- */
493 /* --- ENTITY -------------------------------------------------------- */
494 /* ------------------------------------------------------------------- */
495 
496 void SE3TrajectoryGenerator::display(std::ostream& os) const {
497  os << "SE3TrajectoryGenerator " << getName();
498  try {
499  getProfiler().report_all(3, os);
500  } catch (ExceptionSignal e) {
501  }
502 }
503 } // namespace torque_control
504 } // namespace sot
505 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_initSucceeded
bool m_initSucceeded
Definition: se3-trajectory-generator.hh:140
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::startLinearChirp
void startLinearChirp(const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
Definition: se3-trajectory-generator.cpp:429
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_status
std::vector< TG_Status > m_status
Definition: se3-trajectory-generator.hh:152
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_SPLINE
@ TG_SPLINE
Definition: se3-trajectory-generator.hh:137
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_TRIANGLE
@ TG_TRIANGLE
Definition: se3-trajectory-generator.hh:134
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::stop
void stop(const int &id)
Definition: se3-trajectory-generator.cpp:469
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_LIN_CHIRP
@ TG_LIN_CHIRP
Definition: se3-trajectory-generator.hh:133
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator
Definition: trajectory-generators.hh:287
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_np
unsigned int m_np
control loop time period
Definition: se3-trajectory-generator.hh:143
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_currentTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the component
Definition: se3-trajectory-generator.hh:153
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator
Definition: trajectory-generators.hh:262
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::init
void init(const double &dt)
Definition: se3-trajectory-generator.cpp:106
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator
Definition: trajectory-generators.hh:381
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_triangleTrajGen
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
Definition: se3-trajectory-generator.hh:158
se3-trajectory-generator.hh
dynamicgraph::sot::torque_control::NoTrajectoryGenerator
Definition: trajectory-generators.hh:179
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_dt
double m_dt
true if it is the first iteration, false otherwise
Definition: se3-trajectory-generator.hh:142
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_t
double m_t
last iter index
Definition: se3-trajectory-generator.hh:147
commands-helper.hh
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_noTrajGen
std::vector< NoTrajectoryGenerator * > m_noTrajGen
Definition: se3-trajectory-generator.hh:154
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_firstIter
bool m_firstIter
true if the entity has been successfully initialized
Definition: se3-trajectory-generator.hh:141
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::startConstAcc
void startConstAcc(const int &id, const double &xFinal, const double &time)
Definition: se3-trajectory-generator.cpp:412
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::startTriangle
void startTriangle(const int &id, const double &xFinal, const double &time, const double &Tacc)
Definition: se3-trajectory-generator.cpp:389
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineRotation
Eigen::Matrix3d m_splineRotation
Definition: se3-trajectory-generator.hh:148
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::display
virtual void display(std::ostream &os) const
Definition: se3-trajectory-generator.cpp:496
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineTrajGen
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
Definition: se3-trajectory-generator.hh:149
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::playTrajectoryFile
void playTrajectoryFile(const std::string &fileName)
Definition: se3-trajectory-generator.cpp:276
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_STOP
@ TG_STOP
Definition: se3-trajectory-generator.hh:130
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_linChirpTrajGen
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
Definition: se3-trajectory-generator.hh:157
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::loadTextFile
virtual bool loadTextFile(const std::string &fileName)
Definition: trajectory-generators.hh:202
dynamicgraph::command::docCommandVoid5
std::string docCommandVoid5(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5)
Definition: commands-helper.hh:117
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_MIN_JERK
@ TG_MIN_JERK
Definition: se3-trajectory-generator.hh:132
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_SINUSOID
@ TG_SINUSOID
Definition: se3-trajectory-generator.hh:131
dynamicgraph::sot::torque_control::MinimumJerkTrajectoryGenerator
Definition: trajectory-generators.hh:235
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_minJerkTrajGen
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
Definition: se3-trajectory-generator.hh:155
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_constAccTrajGen
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
Definition: se3-trajectory-generator.hh:159
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::startSinusoid
void startSinusoid(const int &id, const double &xFinal, const double &time)
Definition: se3-trajectory-generator.cpp:372
dynamicgraph::command::makeCommandVoid5
CommandVoid5< E, T1, T2, T3, T4, T5 > * makeCommandVoid5(E &entity, typename CommandVoid5< E, T1, T2, T3, T4, T5 >::function_t function, const std::string &docString)
Definition: commands-helper.hh:94
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator
Definition: trajectory-generators.hh:193
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_sinTrajGen
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
Definition: se3-trajectory-generator.hh:156
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::SE3TrajectoryGenerator
SE3TrajectoryGenerator(const std::string &name)
Definition: se3-trajectory-generator.cpp:34
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator
Definition: trajectory-generators.hh:334
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::getValue
void getValue(const int &id)
Definition: se3-trajectory-generator.cpp:269
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_TEXT_FILE
@ TG_TEXT_FILE
Definition: se3-trajectory-generator.hh:136
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineReady
bool m_splineReady
Definition: se3-trajectory-generator.hh:150
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::move
void move(const int &id, const double &xFinal, const double &time)
Definition: se3-trajectory-generator.cpp:454
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::get_initial_point
virtual const Eigen::VectorXd & get_initial_point()
Definition: trajectory-generators.hh:170
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_CONST_ACC
@ TG_CONST_ACC
Definition: se3-trajectory-generator.hh:135
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::setSpline
void setSpline(const std::string &filename, const double &timeToInitConf, const Eigen::MatrixXd &init_rotation)
Definition: se3-trajectory-generator.cpp:314
PROFILE_SE3_POSITION_DESIRED_COMPUTATION
#define PROFILE_SE3_POSITION_DESIRED_COMPUTATION
Definition: se3-trajectory-generator.cpp:22
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_textFileTrajGen
TextFileTrajectoryGenerator * m_textFileTrajGen
Definition: se3-trajectory-generator.hh:160
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::startSpline
void startSpline()
Definition: se3-trajectory-generator.cpp:364