sot-talos-balance  1.6.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("set",
131  makeCommandVoid2(*this, &NdTrajectoryGenerator::set,
132  docCommandVoid2("Instantaneously set component corresponding to index to the specified value.",
133  "(int) index",
134  "(double) desired value")));
135  addCommand("stop",
136  makeCommandVoid1(*this, &NdTrajectoryGenerator::stop,
137  docCommandVoid1("Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
138  "(int) index")));
139 
140  }
141 
142  void NdTrajectoryGenerator::init(const double& dt, const unsigned int& n)
143  {
144  if(dt<=0.0)
145  return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
146  if(n<1)
147  return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
148  m_firstIter = true;
149  m_dt = dt;
150  m_n = n;
151  m_status.resize(m_n,JTG_STOP);
152  m_minJerkTrajGen.resize(m_n);
153  m_sinTrajGen.resize(m_n);
154  //m_triangleTrajGen.resize(m_n);
155  m_constAccTrajGen.resize(m_n);
156  m_linChirpTrajGen.resize(m_n);
157  m_currentTrajGen.resize(m_n);
158  m_noTrajGen.resize(m_n);
159  for(unsigned i=0; i<m_n; i++)
160  {
161  m_minJerkTrajGen[i] = new parametriccurves::MinimumJerk<double,1>(5.0);
162  m_sinTrajGen[i] = new parametriccurves::InfiniteSinusoid<double,1>(5.0);
163  //m_triangleTrajGen[i] = new parametriccurves::InfiniteTriangle<double,1>(5.0);
164  m_constAccTrajGen[i] = new parametriccurves::InfiniteConstAcc<double,1>(5.0);
165  m_linChirpTrajGen[i] = new parametriccurves::LinearChirp<double,1>(5.0);
166  m_noTrajGen[i] = new parametriccurves::Constant<double,1>(5.0);
168 
169  //Set infinite time for infinite trajectories
170  m_noTrajGen[i]->setTimePeriod(DOUBLE_INF);
171  m_constAccTrajGen[i]->setTimePeriod(DOUBLE_INF);
172  m_sinTrajGen[i]->setTimePeriod(DOUBLE_INF);
173  }
174  m_splineTrajGen = new parametriccurves::Spline<double,Eigen::Dynamic>();
175  m_textFileTrajGen = new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
176 
177  NdTrajectoryGenerator::setLoggerVerbosityLevel(dynamicgraph::LoggerVerbosity::VERBOSITY_ALL);
178  m_initSucceeded = true;
179  }
180 
181  /* ------------------------------------------------------------------- */
182  /* --- SIGNALS ------------------------------------------------------- */
183  /* ------------------------------------------------------------------- */
184 
186  {
187  if(!m_initSucceeded)
188  {
189  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
190  return s;
191  }
192 
193  getProfiler().start(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
194  {
195  if(s.size()!=m_n)
196  s.resize(m_n);
197 
198  // at first iteration store initial values
199  if(m_firstIter)
200  {
201  const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
202  if(initial_value.size()!=m_n)
203  {
204  SEND_MSG("Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
205  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
206  return s;
207  }
208  for(unsigned int i=0; i<m_n; i++)
209  m_currentTrajGen[i]->setInitialPoint(initial_value(i));
210  m_firstIter = false;
211  }
212  else if(iter == static_cast<int>(m_iterLast))
213  {
214  if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
215  if(m_status[0]==JTG_TEXT_FILE)
216  {
217  s = (*m_textFileTrajGen)(m_t);
218  }
219  else if(m_status[0]==JTG_SPLINE)
220  {
221  s = (*m_splineTrajGen)(m_t);
222  }
223  else
224  for(unsigned int i=0; i<m_n; i++)
225  s(i) = (*m_currentTrajGen[i])(m_t)[0];
226  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
227  return s;
228  }
229  m_iterLast = iter;
230 
231  const bool & isTriggered = m_triggerSIN(iter);
232 
233  if(isTriggered || m_status[0]!=JTG_TEXT_FILE)
234  m_t += m_dt;
235 
236  if (isTriggered && m_splineReady)
237  startSpline();
238 
239  if(m_status[0]==JTG_TEXT_FILE)
240  {
241  if(!isTriggered)
242  {
243  for(unsigned int i=0; i<m_n; i++)
244  {
245  s(i) = (*m_noTrajGen[i])(m_t)[0];
246  }
247  }
248  else if(!m_textFileTrajGen->checkRange(m_t))
249  {
250  s = (*m_textFileTrajGen)(m_textFileTrajGen->tmax()-m_dt); // HACK!!! m_dt avoids buffer overflow
251  for(unsigned int i=0; i<m_n; i++)
252  {
253  m_noTrajGen[i]->setInitialPoint(s(i));
254  m_status[i] = JTG_STOP;
255  }
256  SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
257  m_t =0;
258  }
259  else
260  s = (*m_textFileTrajGen)(m_t);
261  }
262  else if(m_status[0]==JTG_SPLINE)
263  {
264  if(!m_splineTrajGen->checkRange(m_t))
265  {
266  s = (*m_splineTrajGen)(m_splineTrajGen->tmax());
267  for(unsigned int i=0; i<m_n; i++)
268  {
269  m_noTrajGen[i]->setInitialPoint(s(i));
270  m_status[i] = JTG_STOP;
271  }
272  m_splineReady =false;
273  SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
274  m_t =0;
275  }
276  else
277  s = (*m_splineTrajGen)(m_t);
278  }
279  else
280  {
281  for(unsigned int i=0; i<m_n; i++)
282  {
283  if(!m_currentTrajGen[i]->checkRange(m_t))
284  {
285  s(i) = (*m_currentTrajGen[i])(m_currentTrajGen[i]->tmax())[0];
287  m_noTrajGen[i]->setInitialPoint(s(i));
288  m_status[i] = JTG_STOP;
289  SEND_MSG("Trajectory of index "+toString(i)+" ended.", MSG_TYPE_INFO);
290  }
291  else
292  s(i) = (*m_currentTrajGen[i])(m_t)[0];
293  }
294  }
295  }
296  getProfiler().stop(PROFILE_ND_POSITION_DESIRED_COMPUTATION);
297 
298  return s;
299  }
300 
302  {
303  if(!m_initSucceeded)
304  {
305  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
306  return s;
307  }
308 
309  const dynamicgraph::Vector& x = m_xSOUT(iter);
310  (void) x; // disable unused variable warning
311 
312  if(s.size()!=m_n)
313  s.resize(m_n);
314 
315  const bool & isTriggered = m_triggerSIN(iter);
316 
317  if(m_status[0]==JTG_TEXT_FILE)
318  {
319  if(isTriggered)
320  s = m_textFileTrajGen->derivate(m_t, 1);
321  else
322  s.setZero();
323  }
324  else if(m_status[0]==JTG_SPLINE)
325  s = m_splineTrajGen->derivate(m_t, 1);
326  else
327  for(unsigned int i=0; i<m_n; i++)
328  s(i) = m_currentTrajGen[i]->derivate(m_t, 1)[0];
329 
330  return s;
331  }
332 
334  {
335  if(!m_initSucceeded)
336  {
337  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
338  return s;
339  }
340 
341  const dynamicgraph::Vector& x = m_xSOUT(iter);
342  (void) x; // disable unused variable warning
343 
344  if(s.size()!=m_n)
345  s.resize(m_n);
346 
347  const bool & isTriggered = m_triggerSIN(iter);
348 
349  if(m_status[0]==JTG_TEXT_FILE)
350  {
351  if(isTriggered)
352  s = m_textFileTrajGen->derivate(m_t, 2);
353  else
354  s.setZero();
355  }
356  else if(m_status[0]==JTG_SPLINE)
357  s = m_splineTrajGen->derivate(m_t, 2);
358  else
359  for(unsigned int i=0; i<m_n; i++)
360  s(i) = m_currentTrajGen[i]->derivate(m_t, 2)[0];
361 
362  return s;
363  }
364 
365  /* ------------------------------------------------------------------- */
366  /* --- COMMANDS ------------------------------------------------------ */
367  /* ------------------------------------------------------------------- */
368 
370  {
371  if(id<0 || id>=static_cast<int>(m_n))
372  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
373 
374  SEND_MSG("Current value of component "+toString(id)+" is "+toString( (*m_currentTrajGen[id])(m_t)[0]) , MSG_TYPE_INFO);
375  }
376 
377  void NdTrajectoryGenerator::playTrajectoryFile(const std::string& fileName)
378  {
379  if(!m_initSucceeded)
380  return SEND_MSG("Cannot start trajectory before initialization!",MSG_TYPE_ERROR);
381 
382  for(unsigned int i=0; i<m_n; i++)
383  if(m_status[i]!=JTG_STOP)
384  return SEND_MSG("You cannot control component "+toString(i)+" because it is already controlled.", MSG_TYPE_ERROR);
385 
386  if(!m_textFileTrajGen->loadTextFile(fileName))
387  {
388  std::string msg("Error while loading text file "+fileName);
389  SEND_MSG(msg, MSG_TYPE_ERROR);
390  throw std::runtime_error(msg);
391  }
392 
393  // check current configuration is not too far from initial trajectory configuration
394  bool needToMoveToInitConf = false;
395  const VectorXd& xInit = m_textFileTrajGen->getInitialPoint();
396  for(unsigned int i=0; i<m_n; i++)
397  {
398  const double currentVal = (*m_currentTrajGen[i])(m_t)[0];
399  if(fabs(xInit[i] - currentVal) > 0.001)
400  {
401  needToMoveToInitConf = true;
402  //SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
403  SEND_MSG("Component "+ toString(i) +" is far from initial configuration (" + toString(xInit[i]) + "->" + toString(currentVal) + ")", MSG_TYPE_WARNING);
404  }
405  }
406 /*
407  // if necessary move joints to initial configuration
408  if(needToMoveToInitConf)
409  {
410  for(unsigned int i=0; i<m_n; i++)
411  {
412 // if(!isJointInRange(i, xInit[i]))
413 // return;
414 
415  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
416  m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
417  m_minJerkTrajGen[i]->setTimePeriod(4.0);
418  m_status[i] = JTG_MIN_JERK;
419  m_currentTrajGen[i] = m_minJerkTrajGen[i];
420  }
421  m_t = 0.0;
422  return;
423  }
424 */
425 
426  m_t = 0.0;
427  for(unsigned int i=0; i<m_n; i++)
428  {
429  m_status[i] = JTG_TEXT_FILE;
430  }
431  }
432 
433  void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
434  {
435  if(!m_initSucceeded)
436  return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
437 
438  for(unsigned int i=0; i<m_n; i++)
439  if(m_status[i]!=JTG_STOP)
440  return SEND_MSG("You cannot control component " +toString(i)+" because it is already controlled.",MSG_TYPE_ERROR);
441 
442  if(!m_splineTrajGen->loadFromFile(filename))
443  return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
444 
445  SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
446 
447  bool needToMoveToInitConf = false;
448  if(timeToInitConf < 0)
449  {
450  m_splineReady = true;
451  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
452  return;
453  }
454 
455  // check current configuration is not too far from initial configuration
456  const VectorXd& xInit = (*m_splineTrajGen)(0.0);
457  assert(xInit.size() == m_n);
458  for(unsigned int i=0; i<m_n; i++)
459  if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
460  {
461  needToMoveToInitConf = true;
462  SEND_MSG("Component "+ toString(i) +" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
463  }
464  // if necessary move joints to initial configuration
465  if(needToMoveToInitConf)
466  {
467  for(unsigned int i=0; i<m_n; i++)
468  {
469  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
470  m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
471  m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
472  m_status[i] = JTG_MIN_JERK;
474 // SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
475  }
476  m_t = 0.0;
477  m_splineReady = true;
478  return;
479  }
480  m_splineReady = true;
481  SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
482  }
483 
485  {
486  if(m_status[0]==JTG_SPLINE) return;
487  m_t = 0.0;
488  for(unsigned int i=0; i<m_n; i++)
489  {
490  m_status[i] = JTG_SPLINE;
491  }
492  }
493 
494  void NdTrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, const double& time)
495  {
496  if(!m_initSucceeded)
497  return SEND_MSG("Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
498 
499  if(id<0 || id>=static_cast<int>(m_n))
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", MSG_TYPE_ERROR);
504  if(m_status[i]!=JTG_STOP)
505  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
506 // if(!isJointInRange(i, xFinal))
507 // return;
508 
509  m_sinTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
510  m_sinTrajGen[i]->setFinalPoint(xFinal);
511  m_sinTrajGen[i]->setTrajectoryTime(time);
512  SEND_MSG("Set initial point of sinusoid to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
513  m_status[i] = JTG_SINUSOID;
515  m_t = 0.0;
516  }
517  /*
518  void NdTrajectoryGenerator::startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc)
519  {
520  if(!m_initSucceeded)
521  return SEND_MSG("Cannot start triangle before initialization!",MSG_TYPE_ERROR);
522  if(id<0 || id>=static_cast<int>(m_n))
523  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
524  unsigned int i = id;
525  if(m_status[i]!=JTG_STOP)
526  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
527 // if(!isJointInRange(i, xFinal))
528 // return;
529 
530  m_triangleTrajGen[i]->setInitialPoint(m_noTrajGen[i]->getPos());
531  SEND_MSG("Set initial point of triangular trajectory to "+toString(m_triangleTrajGen[i]->getPos()),MSG_TYPE_DEBUG);
532  m_triangleTrajGen[i]->setFinalPoint(xFinal);
533 
534  if(!m_triangleTrajGen[i]->setTimePeriod(time))
535  return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
536 
537  if(!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
538  return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
539 
540  m_status[i] = JTG_TRIANGLE;
541  m_currentTrajGen[i] = m_triangleTrajGen[i];
542  m_t = 0.0;
543  }
544  */
545  void NdTrajectoryGenerator::startConstAcc(const int& id, const double& xFinal, const double& time)
546  {
547  if(!m_initSucceeded)
548  return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
549  if(id<0 || id>=static_cast<int>(m_n))
550  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
551  unsigned int i = id;
552  if(time<=0.0)
553  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
554  if(m_status[i]!=JTG_STOP)
555  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
556 // if(!isJointInRange(i, xFinal))
557 // return;
558 
559  m_constAccTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
560  SEND_MSG("Set initial point of const-acc trajectory to "+toString((*m_noTrajGen[i])(m_t)[0]),MSG_TYPE_DEBUG);
561  m_constAccTrajGen[i]->setFinalPoint(xFinal);
562  m_constAccTrajGen[i]->setTrajectoryTime(time);
563  m_status[i] = JTG_CONST_ACC;
565  m_t = 0.0;
566  }
567  void NdTrajectoryGenerator::startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time)
568  {
569  if(!m_initSucceeded)
570  return SEND_MSG("Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
571  if(id<0 || id>=static_cast<int>(m_n))
572  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
573  unsigned int i = id;
574  if(time<=0.0)
575  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
576  if(m_status[i]!=JTG_STOP)
577  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
578 // if(!isJointInRange(i, xFinal))
579 // return;
580  if(f0>f1)
581  return SEND_MSG("f0 "+toString(f0)+" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
582  if(f0<=0.0)
583  return SEND_MSG("Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
584 
585  if(!m_linChirpTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]))
586  return SEND_MSG("Error while setting initial point "+toString((*m_noTrajGen[i])(m_t)[0]), MSG_TYPE_ERROR);
587  if(!m_linChirpTrajGen[i]->setFinalPoint(xFinal))
588  return SEND_MSG("Error while setting final point "+toString(xFinal), MSG_TYPE_ERROR);
589  if(!m_linChirpTrajGen[i]->setTimePeriod(time))
590  return SEND_MSG("Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
591  if(!m_linChirpTrajGen[i]->setInitialFrequency(f0))
592  return SEND_MSG("Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
593  if(!m_linChirpTrajGen[i]->setFinalFrequency(f1))
594  return SEND_MSG("Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
595  m_status[i] = JTG_LIN_CHIRP;
597  m_t = 0.0;
598  }
599 
600  void NdTrajectoryGenerator::move(const int& id, const double& xFinal, const double& time)
601  {
602  if(!m_initSucceeded)
603  return SEND_MSG("Cannot move value before initialization!",MSG_TYPE_ERROR);
604  unsigned int i = id;
605  if(id<0 || id>=static_cast<int>(m_n))
606  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
607  if(time<=0.0)
608  return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
609  if(m_status[i]!=JTG_STOP)
610  return SEND_MSG("You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
611 // if(!isJointInRange(i, xFinal))
612 // return;
613 
614  m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
615  m_minJerkTrajGen[i]->setFinalPoint(xFinal);
616  m_minJerkTrajGen[i]->setTimePeriod(time);
617  m_status[i] = JTG_MIN_JERK;
619  m_t = 0.0;
620  }
621 
622  void NdTrajectoryGenerator::set(const int& id, const double& xVal)
623  {
624  if(!m_initSucceeded)
625  return SEND_MSG("Cannot set value before initialization!",MSG_TYPE_ERROR);
626  unsigned int i = id;
627  if(id<0 || id>=static_cast<int>(m_n))
628  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
629  if(m_status[i]!=JTG_STOP)
630  return SEND_MSG("You cannot set the specified component because it is already controlled.", MSG_TYPE_ERROR);
631 
632  m_noTrajGen[i]->setInitialPoint(xVal);
633 
634 // m_status[i] = JTG_STOP;
635 // m_splineReady = false;
636 // m_currentTrajGen[i] = m_noTrajGen[i];
637 // m_t = 0.0;
638  }
639 
640  void NdTrajectoryGenerator::stop(const int& id)
641  {
642  if(!m_initSucceeded)
643  return SEND_MSG("Cannot stop value before initialization!",MSG_TYPE_ERROR);
644 
645  if(id==-1) //Stop entire vector
646  {
647  for(unsigned int i=0; i<m_n; i++)
648  {
649  m_status[i] = JTG_STOP;
650  // update the initial value
651  m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
653  }
654  m_t = 0.0;
655  return;
656  }
657  if(id<0 || id>=static_cast<int>(m_n))
658  return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR);
659  unsigned int i = id;
660  m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
661  m_status[i] = JTG_STOP;
662  m_splineReady = false;
664  m_t = 0.0;
665  }
666 
667  /* ------------------------------------------------------------------- */
668  // ************************ PROTECTED MEMBER METHODS ********************
669  /* ------------------------------------------------------------------- */
670 
671 
672 // bool NdTrajectoryGenerator::isJointInRange(const int& id, double x)
673 // {
674 // JointLimits jl = JointUtil::get_limits_from_id(id);
675 // if(x<jl.lower)
676 // {
677 // SEND_MSG("Desired joint angle is smaller than lower limit: "+toString(jl.lower),MSG_TYPE_ERROR);
678 // return false;
679 // }
680 // if(x>jl.upper)
681 // {
682 // SEND_MSG("Desired joint angle is larger than upper limit: "+toString(jl.upper),MSG_TYPE_ERROR);
683 // return false;
684 // }
685 // return true;
686 // }
687 
688  /* ------------------------------------------------------------------- */
689  /* --- ENTITY -------------------------------------------------------- */
690  /* ------------------------------------------------------------------- */
691 
692  void NdTrajectoryGenerator::display(std::ostream& os) const
693  {
694  os << "NdTrajectoryGenerator "<<getName();
695  try
696  {
697  getProfiler().report_all(3, os);
698  }
699  catch (ExceptionSignal e) {}
700  }
701  } // namespace torquecontrol
702  } // namespace sot
703 } // 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.