sot-talos-balance  1.5.0
nd-trajectory-generator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017,Thomas Flayols, LAAS-CNRS
3  * File derived from sot-torque-control package available on
4  * https://github.com/stack-of-tasks/sot-torque-control
5  *
6  * This file is part of sot-talos-balance.
7  * sot-talos-balance is free software: you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * as published by the Free Software Foundation, either version 3 of
10  * the License, or (at your option) any later version.
11  * sot-torque-control is distributed in the hope that it will be
12  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU Lesser General Public License for more details. You should
15  * have received a copy of the GNU Lesser General Public License along
16  * with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
17  */
18 
20 #include <sot/core/debug.hh>
21 #include <dynamic-graph/factory.h>
22 
24 #include <sot/core/stop-watch.hh>
25 
26 namespace dynamicgraph
27 {
28  namespace sot
29  {
30  namespace talos_balance
31  {
32  namespace dynamicgraph = ::dynamicgraph;
33  using namespace dynamicgraph;
34  using namespace dynamicgraph::command;
35  using namespace std;
36  using namespace Eigen;
37 
38 #define PROFILE_ND_POSITION_DESIRED_COMPUTATION "NdTrajGen: traj computation"
39 #define DOUBLE_INF std::numeric_limits<double>::max()
40  typedef NdTrajectoryGenerator EntityClassName;
43 
44  /* --- DG FACTORY ---------------------------------------------------- */
45  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NdTrajectoryGenerator,
46  "NdTrajectoryGenerator");
47 
48  /* ------------------------------------------------------------------- */
49  /* --- CONSTRUCTION -------------------------------------------------- */
50  /* ------------------------------------------------------------------- */
52  NdTrajectoryGenerator(const std::string& name)
53  : Entity(name)
54  ,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
55  ,CONSTRUCT_SIGNAL_IN(trigger,bool)
56  ,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
57  ,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
58  ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
59  ,m_initSucceeded(false)
60  ,m_firstIter(true)
61  ,m_t(0)
62  ,m_n(1)
63  ,m_iterLast(0)
64  ,m_splineReady(false)
65  {
66  BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
67 
68  Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
69  <<m_triggerSIN);
70 
71  /* Commands. */
72  addCommand("init",
73  makeCommandVoid2(*this, &NdTrajectoryGenerator::init,
74  docCommandVoid2("Initialize the entity.",
75  "Time period in seconds (double)",
76  "size of output vector signal (int)")));
77 
78  addCommand("getValue",
79  makeCommandVoid1(*this, &NdTrajectoryGenerator::getValue,
80  docCommandVoid1("Get the current value of the specified index.",
81  "index (int)")));
82 
83  addCommand("playTrajectoryFile",
84  makeCommandVoid1(*this, &NdTrajectoryGenerator::playTrajectoryFile,
85  docCommandVoid1("Play the trajectory read from the specified text file.",
86  "(string) File name, path included")));
87 
88  addCommand("startSinusoid",
89  makeCommandVoid3(*this, &NdTrajectoryGenerator::startSinusoid,
90  docCommandVoid3("Start an infinite sinusoid motion.",
91  "(int) index",
92  "(double) final value",
93  "(double) time to reach the final value in sec")));
94 
95  addCommand("setSpline",
96  makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
97  docCommandVoid2("Load serialized spline from file",
98  "(string) filename",
99  "(double) time to initial conf")));
100 
101  /* addCommand("startTriangle",
102  makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
103  docCommandVoid4("Start an infinite triangle wave.",
104  "(int) index",
105  "(double) final values",
106  "(double) time to reach the final value in sec",
107  "(double) time to accelerate in sec")));
108  */
109  addCommand("startConstAcc",
110  makeCommandVoid3(*this, &NdTrajectoryGenerator::startConstAcc,
111  docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
112  "(int) index",
113  "(double) final values",
114  "(double) time to reach the final value in sec")));
115 
116  addCommand("startLinChirp",
118  docCommandVoid5("Start a linear-chirp motion.",
119  "(int) index",
120  "(double) final values",
121  "(double) initial frequency [Hz]",
122  "(double) final frequency [Hz]",
123  "(double) trajectory time [sec]")));
124  addCommand("move",
125  makeCommandVoid3(*this, &NdTrajectoryGenerator::move,
126  docCommandVoid3("Move component corresponding to index to the specified value with a minimum jerk trajectory.",
127  "(int) index",
128  "(double) final values",
129  "(double) time to reach the final value in sec")));
130  addCommand("stop",
131  makeCommandVoid1(*this, &NdTrajectoryGenerator::stop,
132  docCommandVoid1("Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
133  "(int) index")));
134 
135  }
136 
137  void NdTrajectoryGenerator::init(const double& dt, const unsigned int& n)
138  {
139  if(dt<=0.0)
140  return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
141  if(n<1)
142  return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
143  m_firstIter = true;
144  m_dt = dt;
145  m_n = n;
146  m_status.resize(m_n,JTG_STOP);
147  m_minJerkTrajGen.resize(m_n);
148  m_sinTrajGen.resize(m_n);
149  //m_triangleTrajGen.resize(m_n);
150  m_constAccTrajGen.resize(m_n);
151  m_linChirpTrajGen.resize(m_n);
152  m_currentTrajGen.resize(m_n);
153  m_noTrajGen.resize(m_n);
154  for(unsigned i=0; i<m_n; i++)
155  {
156  m_minJerkTrajGen[i] = new parametriccurves::MinimumJerk<double,1>(5.0);
157  m_sinTrajGen[i] = new parametriccurves::InfiniteSinusoid<double,1>(5.0);
158  //m_triangleTrajGen[i] = new parametriccurves::InfiniteTriangle<double,1>(5.0);
159  m_constAccTrajGen[i] = new parametriccurves::InfiniteConstAcc<double,1>(5.0);
160  m_linChirpTrajGen[i] = new parametriccurves::LinearChirp<double,1>(5.0);
161  m_noTrajGen[i] = new parametriccurves::Constant<double,1>(5.0);
163 
164  //Set infinite time for infinite trajectories
165  m_noTrajGen[i]->setTimePeriod(DOUBLE_INF);
166  m_constAccTrajGen[i]->setTimePeriod(DOUBLE_INF);
167  m_sinTrajGen[i]->setTimePeriod(DOUBLE_INF);
168  }
169  m_splineTrajGen = new parametriccurves::Spline<double,Eigen::Dynamic>();
170  m_textFileTrajGen = new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
171 
172  NdTrajectoryGenerator::setLoggerVerbosityLevel(dynamicgraph::LoggerVerbosity::VERBOSITY_ALL);
173  m_initSucceeded = true;
174  }
175 
176  /* ------------------------------------------------------------------- */
177  /* --- SIGNALS ------------------------------------------------------- */
178  /* ------------------------------------------------------------------- */
179 
181  {
182  if(!m_initSucceeded)
183  {
184  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
185  return s;
186  }
187 
188  getProfiler().start(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
189  {
190  if(s.size()!=m_n)
191  s.resize(m_n);
192 
193  // at first iteration store initial values
194  if(m_firstIter)
195  {
196  const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
197  if(initial_value.size()!=m_n)
198  {
199  SEND_MSG("Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
200  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
201  return s;
202  }
203  for(unsigned int i=0; i<m_n; i++)
204  m_currentTrajGen[i]->setInitialPoint(initial_value(i));
205  m_firstIter = false;
206  }
207  else if(iter == static_cast<int>(m_iterLast))
208  {
209  if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
210  if(m_status[0]==JTG_TEXT_FILE)
211  {
212  s = (*m_textFileTrajGen)(m_t);
213  }
214  else if(m_status[0]==JTG_SPLINE)
215  {
216  s = (*m_splineTrajGen)(m_t);
217  }
218  else
219  for(unsigned int i=0; i<m_n; i++)
220  s(i) = (*m_currentTrajGen[i])(m_t)[0];
221  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
222  return s;
223  }
224  m_iterLast = iter;
225 
226  const bool & isTriggered = m_triggerSIN(iter);
227 
228  if(isTriggered || m_status[0]!=JTG_TEXT_FILE)
229  m_t += m_dt;
230 
231  if (isTriggered && m_splineReady)
232  startSpline();
233 
234  if(m_status[0]==JTG_TEXT_FILE)
235  {
236  if(!isTriggered)
237  {
238  for(unsigned int i=0; i<m_n; i++)
239  {
240  s(i) = (*m_noTrajGen[i])(m_t)[0];
241  }
242  }
243  else if(!m_textFileTrajGen->checkRange(m_t))
244  {
245  s = (*m_textFileTrajGen)(m_textFileTrajGen->tmax()-m_dt); // HACK!!! m_dt avoids buffer overflow
246  for(unsigned int i=0; i<m_n; i++)
247  {
248  m_noTrajGen[i]->setInitialPoint(s(i));
249  m_status[i] = JTG_STOP;
250  }
251  SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
252  m_t =0;
253  }
254  else
255  s = (*m_textFileTrajGen)(m_t);
256  }
257  else if(m_status[0]==JTG_SPLINE)
258  {
259  if(!m_splineTrajGen->checkRange(m_t))
260  {
261  s = (*m_splineTrajGen)(m_splineTrajGen->tmax());
262  for(unsigned int i=0; i<m_n; i++)
263  {
264  m_noTrajGen[i]->setInitialPoint(s(i));
265  m_status[i] = JTG_STOP;
266  }
267  m_splineReady =false;
268  SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
269  m_t =0;
270  }
271  else
272  s = (*m_splineTrajGen)(m_t);
273  }
274  else
275  {
276  for(unsigned int i=0; i<m_n; i++)
277  {
278  if(!m_currentTrajGen[i]->checkRange(m_t))
279  {
280  s(i) = (*m_currentTrajGen[i])(m_currentTrajGen[i]->tmax())[0];
282  m_noTrajGen[i]->setInitialPoint(s(i));
283  m_status[i] = JTG_STOP;
284  SEND_MSG("Trajectory of index "+toString(i)+" ended.", MSG_TYPE_INFO);
285  }
286  else
287  s(i) = (*m_currentTrajGen[i])(m_t)[0];
288  }
289  }
290  }
291  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
292 
293  return s;
294  }
295 
297  {
298  if(!m_initSucceeded)
299  {
300  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
301  return s;
302  }
303 
304  const dynamicgraph::Vector& x = m_xSOUT(iter);
305  (void) x; // disable unused variable warning
306 
307  if(s.size()!=m_n)
308  s.resize(m_n);
309 
310  const bool & isTriggered = m_triggerSIN(iter);
311 
312  if(m_status[0]==JTG_TEXT_FILE)
313  {
314  if(isTriggered)
315  s = m_textFileTrajGen->derivate(m_t, 1);
316  else
317  s.setZero();
318  }
319  else if(m_status[0]==JTG_SPLINE)
320  s = m_splineTrajGen->derivate(m_t, 1);
321  else
322  for(unsigned int i=0; i<m_n; i++)
323  s(i) = m_currentTrajGen[i]->derivate(m_t, 1)[0];
324 
325  return s;
326  }
327 
329  {
330  if(!m_initSucceeded)
331  {
332  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
333  return s;
334  }
335 
336  const dynamicgraph::Vector& x = m_xSOUT(iter);
337  (void) x; // disable unused variable warning
338 
339  if(s.size()!=m_n)
340  s.resize(m_n);
341 
342  const bool & isTriggered = m_triggerSIN(iter);
343 
344  if(m_status[0]==JTG_TEXT_FILE)
345  {
346  if(isTriggered)
347  s = m_textFileTrajGen->derivate(m_t, 2);
348  else
349  s.setZero();
350  }
351  else if(m_status[0]==JTG_SPLINE)
352  s = m_splineTrajGen->derivate(m_t, 2);
353  else
354  for(unsigned int i=0; i<m_n; i++)
355  s(i) = m_currentTrajGen[i]->derivate(m_t, 2)[0];
356 
357  return s;
358  }
359 
360  /* ------------------------------------------------------------------- */
361  /* --- COMMANDS ------------------------------------------------------ */
362  /* ------------------------------------------------------------------- */
363 
365  {
366  if(id<0 || id>=static_cast<int>(m_n))
367  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
368 
369  SEND_MSG("Current value of component "+toString(id)+" is "+toString( (*m_currentTrajGen[id])(m_t)[0]) , MSG_TYPE_INFO);
370  }
371 
372  void NdTrajectoryGenerator::playTrajectoryFile(const std::string& fileName)
373  {
374  if(!m_initSucceeded)
375  return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
376 
377  for(unsigned int i=0; i<m_n; i++)
378  if(m_status[i]!=JTG_STOP)
379  return SEND_MSG("You cannot control component "+toString(i)+" because it is already controlled.", MSG_TYPE_ERROR);
380 
381  if(!m_textFileTrajGen->loadTextFile(fileName))
382  return SEND_MSG("Error while loading text file "+fileName, MSG_TYPE_ERROR);
383 
384  // check current configuration is not too far from initial trajectory configuration
385  bool needToMoveToInitConf = false;
386  const VectorXd& xInit = m_textFileTrajGen->getInitialPoint();
387  for(unsigned int i=0; i<m_n; i++)
388  {
389  const double currentVal = (*m_currentTrajGen[i])(m_t)[0];
390  if(fabs(xInit[i] - currentVal) > 0.001)
391  {
392  needToMoveToInitConf = true;
393  //SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
394  SEND_MSG("Component "+ toString(i) +" is far from initial configuration (" + toString(xInit[i]) + "->" + toString(currentVal) + ")", MSG_TYPE_WARNING);
395  }
396  }
397 /*
398  // if necessary move joints to initial configuration
399  if(needToMoveToInitConf)
400  {
401  for(unsigned int i=0; i<m_n; i++)
402  {
403 // if(!isJointInRange(i, xInit[i]))
404 // return;
405 
406  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
407  m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
408  m_minJerkTrajGen[i]->setTimePeriod(4.0);
409  m_status[i] = JTG_MIN_JERK;
410  m_currentTrajGen[i] = m_minJerkTrajGen[i];
411  }
412  m_t = 0.0;
413  return;
414  }
415 */
416 
417  m_t = 0.0;
418  for(unsigned int i=0; i<m_n; i++)
419  {
420  m_status[i] = JTG_TEXT_FILE;
421  }
422  }
423 
424  void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
425  {
426  if(!m_initSucceeded)
427  return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
428 
429  for(unsigned int i=0; i<m_n; i++)
430  if(m_status[i]!=JTG_STOP)
431  return SEND_MSG("You cannot control component " +toString(i)+" because it is already controlled.",MSG_TYPE_ERROR);
432 
433  if(!m_splineTrajGen->loadFromFile(filename))
434  return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
435 
436  SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
437 
438  bool needToMoveToInitConf = false;
439  if(timeToInitConf < 0)
440  {
441  m_splineReady = true;
442  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
443  return;
444  }
445 
446  // check current configuration is not too far from initial configuration
447  const VectorXd& xInit = (*m_splineTrajGen)(0.0);
448  assert(xInit.size() == m_n);
449  for(unsigned int i=0; i<m_n; i++)
450  if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
451  {
452  needToMoveToInitConf = true;
453  SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
454  }
455  // if necessary move joints to initial configuration
456  if(needToMoveToInitConf)
457  {
458  for(unsigned int i=0; i<m_n; i++)
459  {
460  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
461  m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
462  m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
463  m_status[i] = JTG_MIN_JERK;
465 // SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
466  }
467  m_t = 0.0;
468  m_splineReady = true;
469  return;
470  }
471  m_splineReady = true;
472  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
473  }
474 
476  {
477  if(m_status[0]==JTG_SPLINE) return;
478  m_t = 0.0;
479  for(unsigned int i=0; i<m_n; i++)
480  {
481  m_status[i] = JTG_SPLINE;
482  }
483  }
484 
485  void NdTrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time)
486  {
487  if(!m_initSucceeded)
488  return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
489 
490  if(id<0 || id>=static_cast<int>(m_n))
491  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
492  unsigned int i = id;
493  if(time<=0.0)
494  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
495  if(m_status[i]!=JTG_STOP)
496  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
497 // if(!isJointInRange(i, xFinal))
498 // return;
499 
500  m_sinTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
501  m_sinTrajGen[i]->setFinalPoint(xFinal);
502  m_sinTrajGen[i]->setTrajectoryTime(time);
503  SEND_MSG("Set initial point of sinusoid to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
504  m_status[i] = JTG_SINUSOID;
506  m_t = 0.0;
507  }
508  /*
509  void NdTrajectoryGenerator::startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc)
510  {
511  if(!m_initSucceeded)
512  return SEND_MSG("Cannot start triangle before initialization!",MSG_TYPE_ERROR);
513  if(id<0 || id>=static_cast<int>(m_n))
514  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
515  unsigned int i = id;
516  if(m_status[i]!=JTG_STOP)
517  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
518 // if(!isJointInRange(i, xFinal))
519 // return;
520 
521  m_triangleTrajGen[i]->setInitialPoint(m_noTrajGen[i]->getPos());
522  SEND_MSG("Set initial point of triangular trajectory to "+toString(m_triangleTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
523  m_triangleTrajGen[i]->setFinalPoint(xFinal);
524 
525  if(!m_triangleTrajGen[i]->setTimePeriod(time))
526  return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
527 
528  if(!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
529  return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
530 
531  m_status[i] = JTG_TRIANGLE;
532  m_currentTrajGen[i] = m_triangleTrajGen[i];
533  m_t = 0.0;
534  }
535  */
536  void NdTrajectoryGenerator::startConstAcc(const int& id, const double& xFinal, const double& time)
537  {
538  if(!m_initSucceeded)
539  return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
540  if(id<0 || id>=static_cast<int>(m_n))
541  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
542  unsigned int i = id;
543  if(time<=0.0)
544  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
545  if(m_status[i]!=JTG_STOP)
546  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
547 // if(!isJointInRange(i, xFinal))
548 // return;
549 
550  m_constAccTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
551  SEND_MSG("Set initial point of const-acc trajectory to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
552  m_constAccTrajGen[i]->setFinalPoint(xFinal);
553  m_constAccTrajGen[i]->setTrajectoryTime(time);
554  m_status[i] = JTG_CONST_ACC;
556  m_t = 0.0;
557  }
558  void NdTrajectoryGenerator::startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time)
559  {
560  if(!m_initSucceeded)
561  return SEND_MSG("Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
562  if(id<0 || id>=static_cast<int>(m_n))
563  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
564  unsigned int i = id;
565  if(time<=0.0)
566  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
567  if(m_status[i]!=JTG_STOP)
568  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
569 // if(!isJointInRange(i, xFinal))
570 // return;
571  if(f0>f1)
572  return SEND_MSG("f0 "+toString(f0)+" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
573  if(f0<=0.0)
574  return SEND_MSG("Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
575 
576  if(!m_linChirpTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]))
577  return SEND_MSG("Error while setting initial point "+toString((*m_noTrajGen[i])(m_t)[0]), MSG_TYPE_ERROR);
578  if(!m_linChirpTrajGen[i]->setFinalPoint(xFinal))
579  return SEND_MSG("Error while setting final point "+toString(xFinal), MSG_TYPE_ERROR);
580  if(!m_linChirpTrajGen[i]->setTimePeriod(time))
581  return SEND_MSG("Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
582  if(!m_linChirpTrajGen[i]->setInitialFrequency(f0))
583  return SEND_MSG("Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
584  if(!m_linChirpTrajGen[i]->setFinalFrequency(f1))
585  return SEND_MSG("Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
586  m_status[i] = JTG_LIN_CHIRP;
588  m_t = 0.0;
589  }
590 
591  void NdTrajectoryGenerator::move(const int& id, const double& xFinal, const double& time)
592  {
593  if(!m_initSucceeded)
594  return SEND_MSG("Cannot move value before initialization!",MSG_TYPE_ERROR);
595  unsigned int i = id;
596  if(id<0 || id>=static_cast<int>(m_n))
597  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
598  if(time<=0.0)
599  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
600  if(m_status[i]!=JTG_STOP)
601  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
602 // if(!isJointInRange(i, xFinal))
603 // return;
604 
605  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
606  m_minJerkTrajGen[i]->setFinalPoint(xFinal);
607  m_minJerkTrajGen[i]->setTimePeriod(time);
608  m_status[i] = JTG_MIN_JERK;
610  m_t = 0.0;
611  }
612 
613 
614  void NdTrajectoryGenerator::stop(const int& id)
615  {
616  if(!m_initSucceeded)
617  return SEND_MSG("Cannot stop value before initialization!",MSG_TYPE_ERROR);
618 
619  if(id==-1) //Stop entire vector
620  {
621  for(unsigned int i=0; i<m_n; i++)
622  {
623  m_status[i] = JTG_STOP;
624  // update the initial value
625  m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
627  }
628  m_t = 0.0;
629  return;
630  }
631  if(id<0 || id>=static_cast<int>(m_n))
632  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
633  unsigned int i = id;
634  m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
635  m_status[i] = JTG_STOP;
636  m_splineReady = false;
638  m_t = 0.0;
639  }
640 
641  /* ------------------------------------------------------------------- */
642  // ************************ PROTECTED MEMBER METHODS ********************
643  /* ------------------------------------------------------------------- */
644 
645 
646 // bool NdTrajectoryGenerator::isJointInRange(const int& id, double x)
647 // {
648 // JointLimits jl = JointUtil::get_limits_from_id(id);
649 // if(x<jl.lower)
650 // {
651 // SEND_MSG("Desired joint angle is smaller than lower limit: "+toString(jl.lower),MSG_TYPE_ERROR);
652 // return false;
653 // }
654 // if(x>jl.upper)
655 // {
656 // SEND_MSG("Desired joint angle is larger than upper limit: "+toString(jl.upper),MSG_TYPE_ERROR);
657 // return false;
658 // }
659 // return true;
660 // }
661 
662  /* ------------------------------------------------------------------- */
663  /* --- ENTITY -------------------------------------------------------- */
664  /* ------------------------------------------------------------------- */
665 
666  void NdTrajectoryGenerator::display(std::ostream& os) const
667  {
668  os << "NdTrajectoryGenerator "<<getName();
669  try
670  {
671  getProfiler().report_all(3, os);
672  }
673  catch (ExceptionSignal e) {}
674  }
675  } // namespace torquecontrol
676  } // namespace sot
677 } // namespace dynamicgraph
void startSinusoid(const int &id, const double &xFinal, const double &time)
std::vector< parametriccurves::LinearChirp< double, 1 > * > m_linChirpTrajGen
std::vector< parametriccurves::InfiniteConstAcc< double, 1 > *> m_constAccTrajGen
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
STL namespace.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_ND_POSITION_DESIRED_COMPUTATION
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
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)
double m_dt
true if it is the first iteration, false otherwise
std::vector< parametriccurves::InfiniteSinusoid< double, 1 > *> m_sinTrajGen
std::vector< parametriccurves::AbstractCurve< double, dynamicgraph::sot::Vector1d > *> m_currentTrajGen
status of the component
parametriccurves::TextFile< double, Eigen::Dynamic > * m_textFileTrajGen
void move(const int &id, const double &xFinal, const double &time)
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)
void init(const double &dt, const unsigned int &n)
void startConstAcc(const int &id, const double &xFinal, const double &time)
#define DOUBLE_INF
void setSpline(const std::string &filename, const double &timeToInitConf)
void startLinearChirp(const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
std::vector< parametriccurves::MinimumJerk< double, 1 > *> m_minJerkTrajGen
std::vector< parametriccurves::Constant< double, 1 > *> m_noTrajGen
bool m_firstIter
true if the entity has been successfully initialized
std::vector< JTG_Status > m_status
true if the spline has been successfully loaded.