Trajectory.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2016
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #include "Trajectory.h"
26 #include "TrajectoryController.h"
27 #include "VectorHelpers.h"
29 #include "math/MathUtils.h"
30 #include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
31 
32 #include <memory>
33 #include <cmath>
34 
35 namespace armarx
36 {
37 
39  = default;
40 
42  {
43  dataVec.clear();
44  timestamps.clear();
45  dataVec.reserve(dim());
46  for (const auto& it : *this)
47  {
48  std::vector< DoubleSeqPtr >& data = it.data;
49  DoubleSeqSeq new2DVec;
50  new2DVec.reserve(data.size());
51  for (DoubleSeqPtr& subVec : data)
52  {
53  if (subVec)
54  {
55  new2DVec.emplace_back(*subVec);
56  }
57  else
58  {
59  new2DVec.emplace_back(Ice::DoubleSeq());
60  }
61  }
62  dataVec.emplace_back(new2DVec);
63  }
64  timestamps = getTimestamps();
65  }
66 
68  {
69  int i = 0;
70  dataMap.clear();
71  for (DoubleSeqSeq& _2DVec : dataVec)
72  {
73 
74  double t = timestamps.at(i);
75  int d = 0;
76  for (auto& vec : _2DVec)
77  {
78  setEntries(t, d++, vec);
79  }
80 
81  i++;
82  }
83  }
84 
85  void Trajectory::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
86  {
87  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
88  Ice::StringSeq columnContent;
89  Eigen::MatrixXd m = toEigen();
90  auto cols = m.cols();
91  auto rows = m.rows();
92  for (int col = 0; col < cols; col++)
93  {
94  std::stringstream ss;
95 
96  for (int row = 0; row < rows; row++)
97  {
98  ss << m(row, col) << (row < rows - 1 ? "," : "");
99  }
100 
101  columnContent.push_back(ss.str());
102  }
103  obj->setDoubleArray("timestamps", getTimestamps());
104  obj->setStringArray("dimensionData", columnContent);
105  obj->setStringArray("dimensionNames", dimensionNames);
106  }
107 
108  void Trajectory::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
109  {
110  clear();
111  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
112  Ice::StringSeq rowContent;
113  obj->getStringArray("dimensionData", rowContent);
114 
115  Ice::DoubleSeq timestamps;
116  obj->getDoubleArray("timestamps", timestamps);
117  for (const auto& row : rowContent)
118  {
119  Ice::StringSeq values = Split(row, ",");
120  Ice::DoubleSeq newRow;
121  newRow.reserve(values.size());
122  for (std::string v : values)
123  {
124  newRow.push_back(atof(v.c_str()));
125  }
126  addDimension(newRow, timestamps);
127  }
128  obj->getStringArray("dimensionNames", dimensionNames);
129  }
130 
131  VariantDataClassPtr Trajectory::clone(const Ice::Current&) const
132  {
133  return new Trajectory(*this);
134  }
135 
137  {
138  TrajectoryPtr t = new Trajectory();
139  CopyMetaData(*this, *t);
140  return t;
141  }
142 
143  std::string Trajectory::output(const Ice::Current&) const
144  {
145 
146  const ordered_view& ordv = dataMap.get<TagOrdered>();
147  typename ordered_view::const_iterator itMap = ordv.begin();
148  std::stringstream s;
149  s << "Dimensions names: \n";
150  for (size_t i = 0; i < dimensionNames.size(); i++)
151  {
152  s << dimensionNames.at(i) << (i < limitless.size() && limitless.at(i).enabled ? " Limitless " : "") << "\n";
153  }
154 
155  for (; itMap != ordv.end(); itMap++)
156  {
157  s << *itMap << std::endl;
158  }
159  return s.str();
160  }
161 
162  Ice::Int Trajectory::getType(const Ice::Current&) const
163  {
165  }
166 
167  bool Trajectory::validate(const Ice::Current&)
168  {
169  return true;
170  }
171 
173  {
174  return clone();
175  }
176 
178  IceUtil::Shared(source),
179  armarx::Serializable(source),
180  armarx::VariantDataClass(source),
181  TrajectoryBase(source)
182  {
183  CopyData(source, *this);
184  }
185 
186 
187 
188  Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq >& data)
189  {
190  if (data.size() == 0)
191  {
192  return;
193  }
194 
195  typename std::map<double, Ice::DoubleSeq >::const_iterator it = data.begin();
196 
197  for (; it != data.end(); it++)
198  {
199  TrajData dataEntry(this);
200  dataEntry.timestamp = it->first;
201  const Ice::DoubleSeq& dataVec = it->second;
202 
203  for (double i : dataVec)
204  {
205  checkValue(i);
206  dataEntry.data.push_back(std::make_shared<Ice::DoubleSeq>(1, i));
207  }
208  dataMap.insert(std::move(dataEntry));
209  }
210  }
211 
212  //
213  // Trajectory::Trajectory(const TrajMap &map)
214  // {
215  // copyMap(map, dataMap);
216  // }
217 
219  {
220  CopyData(source, *this);
221  return *this;
222  }
223 
224 
225  double Trajectory::getState(double t, size_t dim, size_t derivation)
226  {
227  if (dim >= this->dim())
228  {
229  throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
230  }
231 
232  typename timestamp_view::iterator it = dataMap.find(t);
233 
234  if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
235  {
236  // ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
237  __fillBaseDataAtTimestamp(t);// interpolates and retrieves
238  it = dataMap.find(t);
239  }
240 
241  if (it->data.size() <= dim)
242  {
243  // ARMARX_ERROR << "FAILED!";
244  // ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
245  throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
246  }
247 
248  if (!it->data.at(dim))
249  // it->data.at(dim).reset(new Ice::DoubleSeq());
250  {
251  throw LocalException() << "std::vector ptr is NULL!?";
252  }
253 
254 
255 
256  std::vector<DoubleSeqPtr>& vec = it->data;
257  ARMARX_CHECK_GREATER(vec.size(), dim);
258  if (derivation != 0 && vec.at(dim)->size() <= derivation)
259  {
260  //resize and calculate derivations
261  ARMARX_CHECK_GREATER(vec.size(), dim);
262  ARMARX_CHECK_EXPRESSION(vec.at(dim));
263 
264  size_t curDeriv = vec.at(dim)->size();
265  // ARMARX_INFO << VAROUT(curDeriv) << VAROUT(dim);
266  vec.at(dim)->resize(derivation + 1);
267 
268  while (curDeriv <= derivation)
269  {
270  double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
271  checkValue(curDeriv);
272  vec.at(dim)->at(curDeriv) = derivValue;
273  curDeriv++;
274  }
275 
276  }
277 
278  ARMARX_CHECK_GREATER(vec.at(dim)->size(), derivation) << VAROUT(t) << VAROUT(dim) << VAROUT(*this);
279  // std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
280  double result = vec.at(dim)->at(derivation);
281  // checkValue(result);
282  return result;
283  }
284 
285  double Trajectory::getState(double t, size_t dim, size_t derivation) const
286  {
287 
288  if (dim >= this->dim())
289  {
290  throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
291  }
292 
293  typename timestamp_view::iterator it = dataMap.find(t);
294 
295  if (!dataExists(t, dim, derivation))
296  {
297  if (derivation == 0)
298  {
299  return __interpolate(t, dim, derivation);
300  }
301  else
302  {
303  return getDiscretDifferentiationForDimAtT(t, dim, derivation);
304  }
305  }
306  else
307  {
308  // std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
309  double result = it->data.at(dim)->at(derivation);
310 
311  checkValue(result);
312 
313 
314  return result;
315  }
316  }
317 
318  std::vector<Ice::DoubleSeq> Trajectory::getAllStates(double t, int maxDeriv)
319  {
320  std::vector<Ice::DoubleSeq> res;
321 
322  for (size_t i = 0; i < dim(); i++)
323  {
324  Ice::DoubleSeq curdata;
325 
326  for (int deri = 0; deri <= maxDeriv; deri++)
327  {
328  curdata.push_back(getState(t, i, deri));
329  }
330 
331  res.push_back(curdata);
332  }
333 
334  return res;
335  }
336 
337  Ice::DoubleSeq Trajectory::getDerivations(double t, size_t dimension, size_t derivations) const
338  {
339  if (dimension >= dim())
340  {
341  throw LocalException() << "Dimension out of bounds: requested: " << dimension << " available: " << dim();
342  }
343 
344  Ice::DoubleSeq result;
345 
346  for (size_t i = 0; i <= derivations; i++)
347  {
348  result.push_back(getState(t, dimension, i));
349  }
350 
351  return result;
352  }
353 
354  std::string Trajectory::getDimensionName(size_t dim) const
355  {
356  return dimensionNames.at(dim);
357  }
358 
359  const Ice::StringSeq& Trajectory::getDimensionNames() const
360  {
361  return dimensionNames;
362  }
363 
365  {
366  return dataMap;
367  }
368 
369  Ice::DoubleSeq
370  Trajectory::getDimensionData(size_t dimension, size_t derivation) const
371  {
372  if (dimension >= dim())
373  {
374  throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
375  }
376 
377  Ice::DoubleSeq result;
378 
379  const ordered_view& ordv = dataMap.get<TagOrdered>();
380  typename ordered_view::const_iterator itMap = ordv.begin();
381 
382  for (; itMap != ordv.end(); itMap++)
383  {
384  DoubleSeqPtr dataPtr = itMap->data[dimension];
385  result.push_back(itMap->getDeriv(dimension, derivation));
386  // if(dataPtr && dataPtr->size() > derivation)
387  // result.push_back(itMap->getDeriv(dimension, derivation));
388  // else result.push_back(getState(itMap->timestamp, dimension, derivation));
389  }
390 
391  return result;
392 
393  }
394 
395  Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation) const
396  {
397  return getDimensionDataAsEigen(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
398  }
399 
400  Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const
401  {
402  if (dimension >= dim())
403  {
404  throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
405  }
406 
407  Ice::DoubleSeq result;
408 
409  const ordered_view& ordv = dataMap.get<TagOrdered>();
410  ordered_view::iterator itO = ordv.lower_bound(startTime);
411  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
412  // typename ordered_view::const_iterator itMap = ordv.begin();
413 
414  size_t i = 0;
415  for (; itO != itOEnd && i < size(); itO++, i++)
416  {
417  DoubleSeqPtr dataPtr = itO->data[dimension];
418  result.push_back(itO->getDeriv(dimension, derivation));
419  // if(dataPtr && dataPtr->size() > derivation)
420  // result.push_back(itMap->getDeriv(dimension, derivation));
421  // else result.push_back(getState(itMap->timestamp, dimension, derivation));
422  }
423  Eigen::VectorXd resultVec(result.size());
424  for (size_t i = 0; i < result.size(); i++)
425  {
426  resultVec(i) = result[i];
427  }
428  return resultVec;
429  }
430 
431  Eigen::MatrixXd Trajectory::toEigen(size_t derivation, double startTime, double endTime) const
432  {
433  Eigen::MatrixXd result(size(), dim());
434 
435  const ordered_view& ordv = dataMap.get<TagOrdered>();
436  ordered_view::iterator itO = ordv.lower_bound(startTime);
437  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
438  // typename ordered_view::const_iterator itMap = ordv.begin();
439 
440  size_t i = 0;
441  for (; itO != itOEnd; itO++, i++)
442  {
443  // DoubleSeqPtr dataPtr = itO->data[dimension];
444  for (size_t d = 0; d < itO->data.size(); d++)
445  {
446  result(i, d) = (itO->getDeriv(d, derivation));
447  }
448  }
449 
450  return result;
451  }
452 
453  Eigen::MatrixXd Trajectory::toEigen(size_t derivation) const
454  {
455  if (size() == 0 || dim() == 0)
456  {
457  return Eigen::MatrixXd();
458  }
459  return toEigen(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
460  }
461 
462  TrajectoryPtr Trajectory::getPart(double startTime, double endTime, size_t numberOfDerivations) const
463  {
464  TrajectoryPtr result(new Trajectory());
465 
466  const ordered_view& ordv = dataMap.get<TagOrdered>();
467  ordered_view::iterator itO = ordv.lower_bound(startTime);
468  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
469  // typename ordered_view::const_iterator itMap = ordv.begin();
470 
471  size_t i = 0;
472  for (; itO != itOEnd; itO++, i++)
473  {
474  // DoubleSeqPtr dataPtr = itO->data[dimension];
475  for (size_t d = 0; d < itO->data.size(); d++)
476  {
477  Ice::DoubleSeq derivs;
478  for (size_t i = 0; i < numberOfDerivations + 1; i++)
479  {
480  derivs.push_back(itO->getDeriv(d, i));
481  }
482 
483  result->addDerivationsToDimension(d, itO->getTimestamp(), derivs);
484  }
485  // result.addPositionsToDimension(d, itO->getTimestamp(), itO->getDeriv(d, 0));
486  }
487  result->setDimensionNames(getDimensionNames());
488  return result;
489  }
490 
491  size_t Trajectory::dim() const
492  {
493  if (dataMap.size() == 0)
494  {
495  return 0;
496  }
497  else
498  {
499  return dataMap.begin()->data.size();
500  }
501  }
502 
503  size_t Trajectory::size() const
504  {
505  return dataMap.size();
506  }
507 
508 
509 
511  {
512  const ordered_view& ordView = dataMap.get<TagOrdered>();
513 
514  if (ordView.begin() != ordView.end())
515  {
516  return ordView.rbegin()->timestamp - ordView.begin()->timestamp;
517  }
518  else
519  {
520  return 0;
521  }
522  }
523 
524  double Trajectory::getLength(size_t derivation) const
525  {
526  if (size() == 0)
527  {
528  return 0;
529  }
530  return getLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
531  }
532 
533  double Trajectory::getLength(size_t derivation, double startTime, double endTime) const
534  {
535  double length = 0.0;
536  const ordered_view& ordv = dataMap.get<TagOrdered>();
537  ordered_view::const_iterator itO = ordv.lower_bound(startTime);
538  ordered_view::const_iterator itOEnd = ordv.upper_bound(endTime);
539  if (itO == ordv.end())
540  {
541  return 0.0;
542  }
543  size_t dimensions = dim();
544  Ice::DoubleSeq prevValue(dimensions);
545  for (size_t d = 0; d < dimensions; ++d)
546  {
547  prevValue[d] = getState(startTime, d, derivation);
548  }
549 
550  itO++;
551 
552  double segmentLength = 0;
553  for (;
554  itO != itOEnd && itO != ordv.end();
555  itO++
556  )
557  {
558  if (itO->getTimestamp() >= endTime)
559  {
560  break;
561  }
562  segmentLength = 0;
563  double value;
564  for (size_t d = 0; d < dimensions; ++d)
565  {
566  value = itO->getDeriv(d, derivation);
567  double diff = value - prevValue[d];
568  segmentLength += diff * diff;
569  prevValue[d] = value;
570  }
571  length += sqrt(segmentLength);
572 
573  }
574  segmentLength = 0;
575  // interpolate end values
576  for (size_t d = 0; d < dimensions; ++d)
577  {
578  double value = getState(endTime, d, derivation);
579  double diff = value - prevValue[d];
580  segmentLength += diff * diff;
581  prevValue[d] = value;
582  }
583  length += sqrt(segmentLength);
584  return length;
585  }
586 
587  double Trajectory::getLength(size_t dimension, size_t derivation) const
588  {
589  if (size() == 0)
590  {
591  return 0;
592  }
593  return getLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
594  }
595 
596  double Trajectory::getLength(size_t dimension, size_t derivation, double startTime, double endTime) const
597  {
598  double length = 0.0;
599  const ordered_view& ordv = dataMap.get<TagOrdered>();
600  ordered_view::iterator itO = ordv.lower_bound(startTime);
601  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
602  if (itO == ordv.end())
603  {
604  return 0.0;
605  }
606  double prevValue = getState(startTime, dimension, derivation);
607 
608  for (;
609  itO != itOEnd && itO != ordv.end();
610  itO++
611  )
612  {
613  if (itO->getTimestamp() >= endTime)
614  {
615  break;
616  }
617  length += fabs(prevValue - itO->getDeriv(dimension, derivation));
618  prevValue = itO->getDeriv(dimension, derivation);
619  }
620  length += fabs(prevValue - getState(endTime, dimension, derivation));
621  return length;
622  }
623 
624  double Trajectory::getSquaredLength(size_t dimension, size_t derivation) const
625  {
626  return getSquaredLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
627  }
628 
629  double Trajectory::getSquaredLength(size_t dimension, size_t derivation, double startTime, double endTime) const
630  {
631  double length = 0.0;
632  const ordered_view& ordv = dataMap.get<TagOrdered>();
633  ordered_view::iterator itO = ordv.lower_bound(startTime);
634  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
635  if (itO == ordv.end())
636  {
637  return 0.0;
638  }
639 
640  double prevValue = itO->getDeriv(dimension, derivation);
641 
642  for (;
643  itO != itOEnd;
644  itO++
645  )
646  {
647  length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, derivation), 2.0));
648  prevValue = itO->getDeriv(dimension, derivation);
649  }
650 
651  return length;
652  }
653 
654  double Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
655  {
656  return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
657  }
658 
659  double Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
660  {
661  return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
662  }
663 
664  double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
665  {
666  double bestValue = initValue;
667  const ordered_view& ordv = dataMap.get<TagOrdered>();
668  ordered_view::iterator itO = ordv.lower_bound(startTime);
669  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
670 
671  if (itO == ordv.end())
672  {
673  return bestValue;
674  }
675  for (;
676  itO != itOEnd;
677  itO++
678  )
679  {
680  bestValue = foo(bestValue, itO->getDeriv(dimension, derivation));
681  }
682 
683  return bestValue;
684  }
685 
686  double Trajectory::getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const
687  {
688  return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
689  }
690 
691  Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
692  {
693  Ice::DoubleSeq result;
694  const ordered_view& ordv = dataMap.get<TagOrdered>();
695  ordered_view::iterator itO = ordv.lower_bound(startTime);
696  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
697 
698  if (itO == ordv.end())
699  {
700  return result;
701  }
702  double preValue = itO->getDeriv(dimension, derivation);
703  for (;
704  itO != itOEnd;
705 
706  )
707  {
708  // double t = itO->getTimestamp();
709  double cur = itO->getDeriv(dimension, derivation);
710  itO++;
711  if (itO == ordv.end())
712  {
713  break;
714  }
715  double next = itO->getDeriv(dimension, derivation);
716  if (cur <= preValue && cur <= next)
717  {
718  result.push_back(cur);
719  }
720  preValue = cur;
721  }
722 
723  return result;
724  }
725 
726  Ice::DoubleSeq Trajectory::getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
727  {
728  Ice::DoubleSeq result;
729  const ordered_view& ordv = dataMap.get<TagOrdered>();
730  ordered_view::iterator itO = ordv.lower_bound(startTime);
731  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
732 
733  if (itO == ordv.end())
734  {
735  return result;
736  }
737  double preValue = itO->getDeriv(dimension, derivation);
738  for (;
739  itO != itOEnd;
740 
741  )
742  {
743  // double t = itO->getTimestamp();
744  double cur = itO->getDeriv(dimension, derivation);
745  itO++;
746  if (itO == ordv.end())
747  {
748  break;
749  }
750  double next = itO->getDeriv(dimension, derivation);
751  if (cur <= preValue && cur <= next)
752  {
753  result.push_back(itO->getTimestamp());
754  }
755  preValue = cur;
756  }
757 
758  return result;
759  }
760 
761  Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
762  {
763  Ice::DoubleSeq result;
764  const ordered_view& ordv = dataMap.get<TagOrdered>();
765  ordered_view::iterator itO = ordv.lower_bound(startTime);
766  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
767 
768  if (itO == ordv.end())
769  {
770  return result;
771  }
772  double preValue = itO->getDeriv(dimension, derivation);
773  for (;
774  itO != itOEnd;
775 
776  )
777  {
778  double cur = itO->getDeriv(dimension, derivation);
779  itO++;
780  if (itO == ordv.end())
781  {
782  break;
783  }
784  double next = itO->getDeriv(dimension, derivation);
785  if (cur >= preValue && cur >= next)
786  {
787  result.push_back(cur);
788  }
789  preValue = cur;
790  }
791 
792  return result;
793  }
794 
795  Ice::DoubleSeq Trajectory::getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
796  {
797  Ice::DoubleSeq result;
798  const ordered_view& ordv = dataMap.get<TagOrdered>();
799  ordered_view::iterator itO = ordv.lower_bound(startTime);
800  ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
801 
802  if (itO == ordv.end())
803  {
804  return result;
805  }
806  double preValue = itO->getDeriv(dimension, derivation);
807  for (;
808  itO != itOEnd;
809 
810  )
811  {
812  double cur = itO->getDeriv(dimension, derivation);
813  itO++;
814  if (itO == ordv.end())
815  {
816  break;
817  }
818  double next = itO->getDeriv(dimension, derivation);
819 
820  if (cur >= preValue && cur >= next)
821  {
822  result.push_back(itO->getTimestamp());
823  }
824  preValue = cur;
825  }
826 
827  return result;
828  }
829 
830 
831  std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
832  {
833  // ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
834  // typename timestamp_view::const_iterator it = dataMap.find(t);
835  // if(it != dataMap.end())
836  // return it->data;
837  std::vector<DoubleSeqPtr> result;
838 
839  for (size_t dimension = 0; dimension < dim(); dimension++)
840  {
841  double newValue = __interpolate(t, dimension, 0);
842  checkValue(newValue);
843  result.push_back(std::make_shared<Ice::DoubleSeq>(1, newValue));
844  }
845 
846  return result;
847 
848  }
849 
850  bool Trajectory::dataExists(double t, size_t dimension, size_t derivation) const
851  {
852  typename timestamp_view::iterator it = dataMap.find(t);
853 
854  if (it == dataMap.end() || !it->data.at(dimension) || it->data.at(dimension)->size() <= derivation)
855  {
856  return false;
857  }
858  else
859  {
860  return true;
861  }
862  }
863 
864 
865  Trajectory::timestamp_view::iterator Trajectory::__fillBaseDataAtTimestamp(const double& t)
866  {
867  typename timestamp_view::const_iterator it = dataMap.find(t);
868 
869  if (it != dataMap.end() && it->data.size() == dim())
870  {
871  bool foundEmpty = false;
872 
873  for (auto& i : it->data)
874  {
875  if (!i || i->empty())
876  {
877  foundEmpty = true;
878  break;
879  }
880  }
881 
882  if (!foundEmpty)
883  {
884  // ARMARX_INFO << "Was not empty for " << t;
885  return it;
886  }
887  }
888 
889  TrajData entry(this);
890  entry.timestamp = t;
891  entry.data.resize(dim());
892  dataMap.insert(entry);
893  it = dataMap.find(t);
894 
895  ARMARX_CHECK_EXPRESSION(it != dataMap.end());
896  // const ordered_view& ordv = dataMap.get<TagOrdered>();
897  // typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
898  it->data = __calcBaseDataAtTimestamp(t);
899 
900  return it;
901  }
902 
903  std::vector<DoubleSeqPtr>& Trajectory::getStates(double t)
904  {
905  typename timestamp_view::const_iterator it = dataMap.find(t);
906 
907  if (it == dataMap.end())
908  {
909  // interpolate and insert entry
911  }
912 
913 
914  return it->data;
915  }
916 
917  std::vector<DoubleSeqPtr> Trajectory::getStates(double t) const
918  {
919  typename timestamp_view::const_iterator it = dataMap.find(t);
920 
921  if (it == dataMap.end())
922  {
923  // interpolate and return data
924  return __calcBaseDataAtTimestamp(t);
925  }
926 
927 
928  return it->data;
929  }
930 
931 
932 
933  Ice::DoubleSeq Trajectory::getStates(double t, size_t derivation) const
934  {
935  size_t dimensions = dim();
936  Ice::DoubleSeq result;
937  result.reserve(dimensions);
938  for (size_t i = 0; i < dimensions; i++)
939  {
940  result.push_back(getState(t, i, derivation));
941  }
942 
943  return result;
944  }
945 
946  std::map<double, Ice::DoubleSeq> Trajectory::getStatesAround(double t, size_t derivation, size_t extend) const
947  {
948 
949  std::map<double, Ice::DoubleSeq> res;
950  typename timestamp_view::iterator itMap = dataMap.find(t);
951 
952  if (itMap != dataMap.end())
953  {
954  for (size_t i = 0; i < dim(); i++)
955  {
956  res.insert(std::pair<double, Ice::DoubleSeq>(t, {itMap->data[i]->at(derivation)}));
957  }
958 
959  return res;
960  }
961 
962  const ordered_view& ordv = dataMap.get<TagOrdered>();
963 
964  typename ordered_view::iterator itNext = ordv.upper_bound(t);
965 
966  typename ordered_view::iterator itPrev = itNext;
967 
968  itPrev--;
969 
970  if (itPrev == ordv.end())
971  {
972 
973  throw LocalException("Cannot find value at timestamp ") << t;
974  }
975 
976  for (size_t i = 0; i < extend; i++)
977  {
978  Ice::DoubleSeq preData = getStates(itPrev->timestamp, derivation);
979  Ice::DoubleSeq nexData = getStates(itNext->timestamp, derivation);
980 
981  res.insert(std::pair<double, Ice::DoubleSeq>(itPrev->timestamp, preData));
982  res.insert(std::pair<double, Ice::DoubleSeq>(itNext->timestamp, nexData));
983 
984  if (itPrev == ordv.begin() || itNext == ordv.end())
985  {
986  std::cout << "Warning: the timestamp is out of the range. " <<
987  "The current result will be returned" << std::endl;
988  break;
989  }
990  itPrev--;
991  itNext++;
992  }
993 
994  return res;
995 
996  }
997 
998 
1000  {
1001  size_t dims = traj.dim();
1002  Ice::DoubleSeq timestamps = traj.getTimestamps();
1003 
1004  for (size_t d = 0; d < dims; ++d)
1005  {
1006  addPositionsToDimension(d, timestamps, traj.getDimensionData(d));
1007  }
1008 
1009  return *this;
1010  }
1011 
1012 
1014  {
1015  size_t newDim = dim() + 1;
1016 
1017  typename timestamp_view::iterator itMap = dataMap.begin();
1018 
1019  for (; itMap != dataMap.end(); itMap++)
1020  {
1021  itMap->data.resize(newDim);
1022  }
1023  }
1024 
1025  void Trajectory::setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y)
1026  {
1027  typename timestamp_view::iterator itMap = dataMap.find(t);
1028 
1029  if (itMap == dataMap.end() || dim() == 0)
1030  {
1031  // double dura = getTimeLength();
1032  TrajData newData(this);
1033  newData.timestamp = t;
1034  newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
1035  newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(y);
1036  dataMap.insert(std::move(newData));
1037  }
1038  else
1039  {
1040  assert(dim() > 0);
1041 
1042  while (dim() <= dimIndex)
1043  {
1044  __addDimension();
1045  }
1046 
1047  itMap->data.resize(dim());
1048  ARMARX_CHECK_GREATER(itMap->data.size(), dimIndex);
1049  itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(y);
1050  }
1051 
1052  }
1053 
1054 
1055  void Trajectory::setPositionEntry(const double t, const size_t dimIndex, const double& y)
1056  {
1057  typename timestamp_view::iterator itMap = dataMap.find(t);
1058 
1059  if (itMap == dataMap.end() || dim() == 0)
1060  {
1061  TrajData newData(this);
1062  newData.timestamp = t;
1063  newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
1064  newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(1, y);
1065  dataMap.insert(newData);
1066  }
1067  else
1068  {
1069  assert(dim() > 0);
1070 
1071  while (dim() <= dimIndex)
1072  {
1073  __addDimension();
1074  }
1075 
1076  itMap->data.resize(dim());
1077  itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(1, y);
1078  }
1079 
1080  }
1081 
1082 
1084  {
1085  const ordered_view& ordv = dataMap.get<TagOrdered>();
1086  typename ordered_view::const_iterator itMap = ordv.begin();
1087 
1088  for (; itMap != ordv.end(); itMap++)
1089  {
1090  for (size_t dimension = 0; dimension < dim(); dimension++)
1091  {
1092  if (!itMap->data[dimension])
1093  {
1094  itMap->data[dimension] = std::make_shared<Ice::DoubleSeq>(__interpolate(itMap, dimension, 0));
1095  }
1096  }
1097  }
1098  }
1099 
1100 
1101 
1102 
1103  Ice::DoubleSeq Trajectory::getTimestamps() const
1104  {
1105  Ice::DoubleSeq result;
1106  result.reserve(size());
1107  const ordered_view& ordv = dataMap.get<TagOrdered>();
1108  typename ordered_view::const_iterator itMap = ordv.begin();
1109 
1110  for (; itMap != ordv.end(); itMap++)
1111  {
1112  result.push_back(itMap->timestamp);
1113  }
1114 
1115  return result;
1116  }
1117 
1118  Ice::FloatSeq Trajectory::getTimestampsFloat() const
1119  {
1120  Ice::FloatSeq result;
1121  result.reserve(size());
1122  const ordered_view& ordv = dataMap.get<TagOrdered>();
1123  typename ordered_view::const_iterator itMap = ordv.begin();
1124 
1125  for (; itMap != ordv.end(); itMap++)
1126  {
1127  result.push_back(itMap->timestamp);
1128  }
1129 
1130  return result;
1131  }
1132 
1133 
1134 
1135  Ice::DoubleSeq
1136  Trajectory::getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const
1137  {
1138  if (trajDimension >= dim())
1139  {
1140  throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
1141  }
1142 
1143 
1144  const ordered_view& ordv = dataMap.get<TagOrdered>();
1145  typename ordered_view::const_iterator itPrev = ordv.begin();
1146 
1147  typename ordered_view::const_iterator itCurrent = itPrev;
1148  // itCurrent++;
1149  typename ordered_view::const_iterator itNext = itCurrent;
1150  itNext++;
1151 
1152  Ice::DoubleSeq result;
1153 
1154  for (; itCurrent != ordv.end();)
1155  {
1156  if (itNext == ordv.end()) // last step-> differentiate between current and prev
1157  {
1158  itNext = itCurrent;
1159  }
1160 
1161 
1162  // get current data
1163  DoubleSeqPtr prevStatePtr = itPrev->data[trajDimension];
1164  DoubleSeqPtr nextStatePtr = itNext->data[trajDimension];
1165  // differentiateDiscretly now
1166  result.push_back((nextStatePtr->at(derivation) - prevStatePtr->at(derivation)) /
1167  (itNext->timestamp - itPrev->timestamp));
1168  checkValue(*result.rbegin());
1169 
1170 
1171  itCurrent++, itPrev++, itNext++;
1172 
1173  if (itPrev == itCurrent)
1174  {
1175  // first step-> reestablish that current is > prev
1176  itPrev--;
1177  }
1178 
1179  }
1180 
1181  return result;
1182  }
1183 
1184 
1185  Ice::DoubleSeq Trajectory::DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount)
1186  {
1187  if (derivationCount < 0)
1188  {
1189  throw LocalException("Negative derivation value is not allowed!");
1190  }
1191 
1192  if (derivationCount == 0)
1193  {
1194  return values;
1195  }
1196 
1197  Ice::DoubleSeq result;
1198  int size = std::min(timestamps.size(), values.size());
1199 
1200  if (size < 2)
1201  {
1202  return values;
1203  }
1204 
1205  result.resize(size);
1206 
1207  // boundaries
1208  result[0] = (values.at(1) - values.at(0)) / (timestamps.at(1) - timestamps.at(0)) ;
1209  result[size - 1] = (values.at(size - 1) - values.at(size - 2)) / (timestamps.at(size - 1) - timestamps.at(size - 2)) ;
1210 
1211  //#pragma omp parallel for
1212  for (int i = 1; i < size - 1; ++i)
1213  {
1214  result[i] = (values.at(i + 1) - values.at(i - 1)) / (timestamps.at(i + 1) - timestamps.at(i - 1)) ;
1215  checkValue(result[i]);
1216  }
1217 
1218  if (derivationCount > 1) // recursivly differentiate
1219  {
1220  result = DifferentiateDiscretly(timestamps, result, derivationCount - 1);
1221  }
1222 
1223  return result;
1224 
1225  }
1226 
1227 
1228  double Trajectory::getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const
1229  {
1230  if (derivation == 0)
1231  {
1232  return getState(t, trajDimension, derivation);
1233  }
1234 
1235  typename timestamp_view::iterator it = dataMap.find(t);
1236 
1237  const ordered_view& ordV = dataMap.get<TagOrdered>();
1238  typename ordered_view::iterator itCurrent = ordV.end();
1239 
1240  if (it != dataMap.end())
1241  {
1242  itCurrent = ordV.iterator_to(*it);
1243  }
1244 
1245  typename ordered_view::iterator itNext = ordV.upper_bound(t); // first element after t
1246 
1247  if (it != dataMap.end() && itNext == ordV.end())
1248  {
1249  itNext = itCurrent; // current item is last, set next to current
1250  }
1251  else if (itNext == ordV.end() && it == dataMap.end())
1252  {
1253  throw LocalException() << "Cannot interpolate for t " << t << " no data in trajectory";
1254  }
1255 
1256  typename ordered_view::iterator itPrev = itNext;
1257  itPrev--; // now equal to current (if current exists) or before current
1258 
1259  if (itCurrent != ordV.end()) // check if current item exists
1260  {
1261  itPrev--; //element in front of t
1262  }
1263 
1264  if (itPrev == ordV.end())
1265  {
1266  //element in front of t does not exist
1267  if (itCurrent != ordV.end())
1268  {
1269  itPrev = itCurrent; // set prev element to current
1270  }
1271  else
1272  {
1273  throw LocalException() << "Cannot interpolate for t " << t;
1274  }
1275  }
1276 
1277  if (itNext == ordV.end())
1278  {
1279  //element after t does not exist
1280  if (itCurrent != ordV.end())
1281  {
1282  itNext = itCurrent; // set next element to current
1283  }
1284  else
1285  {
1286  throw LocalException() << "Cannot interpolate for t " << t;
1287  }
1288  }
1289 
1290  if (itNext == itPrev)
1291  {
1292  throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
1293  }
1294 
1295  // double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
1296  double tNext;
1297 
1298  if (dataExists(itNext->timestamp, trajDimension, derivation - 1) || derivation > 1)
1299  {
1300  tNext = itNext->timestamp;
1301  }
1302  else
1303  {
1304  tNext = t;
1305  }
1306 
1307  double next = getState(tNext, trajDimension, derivation - 1);
1308 
1309  double tBefore;
1310 
1311  if (dataExists(itPrev->timestamp, trajDimension, derivation - 1) || derivation > 1)
1312  {
1313  tBefore = itPrev->timestamp;
1314  }
1315  else
1316  {
1317  tBefore = t;
1318  }
1319 
1320  double before = getState(tBefore, trajDimension, derivation - 1);
1321 
1322  if (fabs(tNext - tBefore) < 1e-10)
1323  {
1324  throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(tNext) << VAROUT(tBefore) << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size()) << VAROUT(getTimestamps());
1325  }
1326 
1327  double duration = tNext - tBefore;
1328 
1329  double delta;
1330  if (trajDimension < limitless.size() && limitless.at(trajDimension).enabled)
1331  {
1332  double range = limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
1333 
1334  double dist1 = next - before;
1335  double dist2 = next - (before + range);
1336  double dist3 = next - (before - range);
1337 
1338  if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
1339  {
1340  // no issue with bounds
1341  //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
1342  delta = dist1;
1343  }
1344  else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
1345  {
1346  // go over hi bound
1347  //ARMARX_IMPORTANT << "LIMITLESS deriv HIGH: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
1348  delta = dist2;
1349  }
1350  else
1351  {
1352  // go over lo bound
1353  //ARMARX_IMPORTANT << "LIMITLESS deriv LOW: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
1354  delta = dist3;
1355  }
1356 
1357  //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta is " << delta;
1358  }
1359  else
1360  {
1361  // no limitless joint
1362  delta = next - before;
1363  }
1364 
1365  delta = delta / duration;
1366  //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta/duration is " << delta;
1367  checkValue(delta);
1368  return delta;
1369  }
1370 
1371  void Trajectory::differentiateDiscretly(size_t derivation)
1372  {
1373 
1374  for (size_t d = 0; d < dim(); d++)
1375  {
1376  differentiateDiscretlyForDim(d, derivation);
1377  }
1378  }
1379 
1380  void Trajectory::differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
1381  {
1382  removeDerivation(trajDimension, derivation);
1383  typename ordered_view::iterator itOrd = begin();
1384 
1385  for (; itOrd != end(); itOrd++)
1386  {
1387  getState(itOrd->timestamp, trajDimension, derivation);
1388  }
1389  }
1390 
1391 
1392  void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
1393  {
1394  const ordered_view& ordv = dataMap.get<TagOrdered>();
1395  typename ordered_view::iterator it = ordv.begin();
1396 
1397  if (it == ordv.end())
1398  {
1399  return;
1400  }
1401 
1402  it->data[trajDimension]->at(targetDimOfSystemState) = valueAtFirstTimestamp;
1403  double previousValue = valueAtFirstTimestamp;
1404  double previousTimestamp = it->timestamp;
1405 
1406  // size_t derivation = double().size();
1407  for (it++; it != ordv.end(); it++)
1408  {
1409  double slope;
1410  if (it->data[trajDimension]->size() > sourceDimOfSystemState)
1411  {
1412  slope = it->data[trajDimension]->at(sourceDimOfSystemState);
1413  }
1414  else
1415  {
1416  slope = 0;
1417  }
1418 
1419  double diff = it->timestamp - previousTimestamp;
1420  it->data[trajDimension]->at(targetDimOfSystemState) = previousValue + diff * slope;
1421  previousTimestamp = it->timestamp;
1422  previousValue = it->data[trajDimension]->at(targetDimOfSystemState);
1423  }
1424 
1425  }
1426 
1427 
1428  void Trajectory::negateDim(size_t trajDimension)
1429  {
1430  if (trajDimension >= dim())
1431  {
1432  throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
1433  }
1434 
1435  const ordered_view& ordv = dataMap.get<TagOrdered>();
1436 
1437  Trajectory filteredTraj;
1438 
1439  for (const auto& it : ordv)
1440  {
1441  (*it.data.at(trajDimension)) *= -1;
1442  }
1443  }
1444 
1445 
1446 
1447 
1448 
1449  double
1450  Trajectory::__interpolate(double t, size_t dimension, size_t derivation) const
1451  {
1452  typename timestamp_view::const_iterator it = dataMap.find(t);
1453 
1454  if (it != dataMap.end() && it->data.size() > dimension && it->data.at(dimension) && it->data.at(dimension)->size() > derivation)
1455  {
1456  return it->data.at(dimension)->at(derivation);
1457  }
1458 
1459  const ordered_view& ordv = dataMap.get<TagOrdered>();
1460 
1461  typename ordered_view::iterator itNext = ordv.upper_bound(t);
1462 
1463  typename ordered_view::iterator itPrev = itNext;
1464 
1465  itPrev--;
1466 
1467  double result = __interpolate(t, itPrev, itNext, dimension, derivation);
1468 
1469  checkValue(result);
1470 
1471  return result;
1472  }
1473 
1474  double
1475  Trajectory::__interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const
1476  {
1477  typename ordered_view::iterator itPrev = itMap;
1478  itPrev--;
1479  typename ordered_view::iterator itNext = itMap;
1480  itNext++;
1481  return __interpolate(itMap->timestamp, itPrev, itNext, dimension, derivation);
1482  }
1483 
1484  double
1485  Trajectory::__interpolate(double t, typename ordered_view::const_iterator itPrev, typename ordered_view::const_iterator itNext, size_t dimension, size_t derivation) const
1486  {
1487  const ordered_view& ordView = dataMap.get<TagOrdered>();
1488  double previous = 0;
1489  double next = 0;
1490 
1491  // find previous SystemState that exists for that dimension
1492  while (itPrev != ordView.end() && (itPrev->data.at(dimension) == nullptr || itPrev->data.at(dimension)->size() <= derivation))
1493  {
1494  itPrev--;
1495  }
1496 
1497  if (itPrev != ordView.end())
1498  {
1499  // ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
1500  ARMARX_CHECK_NOT_EQUAL(t, itPrev->timestamp) << VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this);
1501  previous = getState(itPrev->timestamp, dimension, derivation);
1502  }
1503 
1504  // find next SystemState that exists for that dimension
1505  while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
1506  {
1507  itNext++;
1508  }
1509 
1510  if (itNext != ordView.end())
1511  {
1512  // ARMARX_INFO << "Found next state at " << itNext->timestamp;
1513  ARMARX_CHECK_NOT_EQUAL(t, itNext->timestamp) << VAROUT(t) << VAROUT(itNext->timestamp);
1514  next = getState(itNext->timestamp, dimension, derivation);
1515  }
1516 
1517 
1518 
1519  if (itNext == ordView.end() && itPrev == ordView.end())
1520  {
1521  throw LocalException() << "Cannot find next or prev values in dim " << dimension << " at timestamp " << t;
1522  }
1523 
1524  if (itNext == ordView.end())
1525  {
1526  // ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
1527  return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
1528  }
1529  else if (itPrev == ordView.end())
1530  {
1531  // ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
1532  return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
1533  }
1534  else
1535  {
1536  // linear interpolation
1537 
1538  float f = math::MathUtils::ILerp(itPrev->timestamp, itNext->timestamp, t);
1539 
1540  if (dimension < limitless.size() && limitless.at(dimension).enabled)
1541  {
1542  return math::MathUtils::AngleLerp(previous, next, f);
1543  }
1544  else
1545  {
1546  return math::MathUtils::Lerp(previous, next, f);
1547  }
1548  }
1549  }
1550 
1551  void Trajectory::gaussianFilter(double filterRadius)
1552  {
1553  const ordered_view& ordv = dataMap.get<TagOrdered>();
1554  Trajectory filteredTraj;
1555  filteredTraj.setDimensionNames(getDimensionNames());
1556  filteredTraj.setLimitless(getLimitless());
1557  Ice::DoubleSeq timestamps = getTimestamps();
1558 
1559  for (size_t d = 0; d < dim(); d++)
1560  {
1561  Ice::DoubleSeq entries;
1562 
1563  for (typename ordered_view::iterator it = ordv.begin(); it != ordv.end(); it++)
1564  {
1565  entries.push_back(__gaussianFilter(filterRadius, it, d, 0));
1566  }
1567 
1568  filteredTraj.addDimension(entries, timestamps);
1569  }
1570 
1571  CopyData(filteredTraj, *this);
1572  }
1573 
1574  double
1575  Trajectory::__gaussianFilter(double filterRadiusInTime, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
1576  {
1577  const ordered_view& ordView = dataMap.get<TagOrdered>();
1578  const double sigma = filterRadiusInTime / 2.5;
1579 
1580  double weightedSum = 0;
1581  double sumOfWeight = 0;
1582  ordered_view::iterator start = centerIt;
1583 
1584  const double sqrt2PI = sqrt(2 * M_PI);
1585 
1586  for (int sign = -1; sign < 2; sign += 2)
1587  {
1588  for (ordered_view::iterator it = start;
1589  it != ordView.end()
1590  && fabs(it->timestamp - centerIt->timestamp) <= fabs(filterRadiusInTime * 2);
1591  )
1592  {
1593  double value;
1594  value = it->getDeriv(trajNum, dim);//data[trajNum]->at(dim);
1595  double diff = (it->timestamp - centerIt->timestamp);
1596  double squared = diff * diff;
1597  const double gaussValue = exp(-squared / (2 * sigma * sigma)) / (sigma * sqrt2PI);
1598  sumOfWeight += gaussValue;
1599  weightedSum += gaussValue * value;
1600 
1601  if (sign > 0)
1602  {
1603  it++;
1604  }
1605  else
1606  {
1607  it--;
1608  }
1609 
1610  checkValue(weightedSum);
1611  }
1612 
1613  start++;// skip center value in second loop iteration
1614  }
1615 
1616  double result;
1617  result = weightedSum / sumOfWeight;
1618  checkValue(result);
1619  return result;
1620 
1621  }
1622 
1623 
1625  {
1626  if (&source == &destination)
1627  {
1628  return;
1629  }
1630 
1631  destination.clear();
1632 
1633  Ice::DoubleSeq timestamps = source.getTimestamps();
1634 
1635  for (size_t dim = 0; dim < source.dim(); dim++)
1636  {
1637  destination.addDimension(source.getDimensionData(dim), timestamps
1638  );
1639  }
1640  CopyMetaData(source, destination);
1641  }
1642 
1644  {
1645  destination.setDimensionNames(source.getDimensionNames());
1646 
1647  destination.setLimitless(source.getLimitless());
1648  }
1649 
1650 
1651  void Trajectory::clear(bool keepMetaData)
1652  {
1653  dataMap.erase(dataMap.begin(), dataMap.end());
1654  if (!keepMetaData)
1655  {
1656  dimensionNames.clear();
1657  limitless.clear();
1658  }
1659  }
1660 
1661 
1662 
1663  Ice::DoubleSeq Trajectory::GenerateTimestamps(double startTime, double endTime, double stepSize)
1664  {
1665  if (startTime >= endTime)
1666  {
1667  throw LocalException("startTime must be smaller than endTime.");
1668  }
1669 
1670  Ice::DoubleSeq result;
1671  size_t size = std::round((endTime - startTime) / stepSize) + 1;
1672  stepSize = (endTime - startTime) / (size - 1);
1673  result.reserve(size);
1674 
1675  double currentTimestamp = startTime;
1676  size_t i = 0;
1677 
1678  while (i < size)
1679  {
1680  result.push_back(currentTimestamp);
1681  currentTimestamp += stepSize;
1682  i++;
1683  }
1684  ARMARX_CHECK_EQUAL(result.size(), size) << VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize);
1685  return result;
1686  }
1687 
1688 
1689  Ice::DoubleSeq Trajectory::NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
1690  {
1691  Ice::DoubleSeq normTimestamps;
1692  normTimestamps.resize(timestamps.size());
1693  const double minValue = *timestamps.begin();
1694  const double maxValue = *timestamps.rbegin();
1695  const double duration = maxValue - minValue;
1696 
1697  for (size_t i = 0; i < timestamps.size(); i++)
1698  {
1699  normTimestamps[i] = startTime + (timestamps.at(i) - minValue) / duration * (endTime - startTime);
1700  }
1701 
1702  return normTimestamps;
1703  }
1704 
1705 
1706  Trajectory Trajectory::NormalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
1707  {
1708 
1709  if (traj.size() <= 1 || (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
1710  {
1711  return traj; // already normalized
1712  }
1713 
1714 
1715  Ice::DoubleSeq timestamps = traj.getTimestamps();
1716 
1717 
1718  Ice::DoubleSeq normTimestamps = NormalizeTimestamps(timestamps, startTime, endTime);
1719  Trajectory normExampleTraj;
1720 
1721  for (size_t dim = 0; dim < traj.dim(); dim++)
1722  {
1723  Ice::DoubleSeq dimensionData = traj.getDimensionData(dim);
1724  normExampleTraj.addDimension(dimensionData, normTimestamps);
1725  }
1726  normExampleTraj.setDimensionNames(traj.getDimensionNames());
1727  normExampleTraj.setLimitless(traj.getLimitless());
1728  return normExampleTraj;
1729 
1730 
1731  }
1732 
1733  TrajectoryPtr Trajectory::normalize(const double startTime, const double endTime)
1734  {
1735  Trajectory normTraj = NormalizeTimestamps(*this, startTime, endTime);
1736  TrajectoryPtr newTraj = new Trajectory(normTraj);
1737  return newTraj;
1738  }
1739 
1740  TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, const IceUtil::Time& timestep)
1741  {
1742  Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(dim(), maxVelocity);
1743  Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(dim(), maxAcceleration);
1744  return calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, timestep);
1745  }
1746 
1747  TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep)
1748  {
1749 
1750  bool hasLimitlessDimension = false;
1751  for (auto l : limitless)
1752  {
1753  hasLimitlessDimension |= l.enabled;
1754  }
1755 
1756  TrajectoryPtr unfoldedTraj;
1757  if (hasLimitlessDimension)
1758  {
1759  unfoldedTraj = new Trajectory(*this);
1761  }
1762 
1763 
1764  auto timestepValue = timestep.toSecondsDouble();
1765  std::list<Eigen::VectorXd> waypointList;
1766  auto dimensions = dim();
1767  for (auto& waypoint : hasLimitlessDimension ? *unfoldedTraj : *this)
1768  {
1769  auto positions = waypoint.getPositions();
1770  waypointList.push_back(Eigen::Map<Eigen::VectorXd>(positions.data(), dimensions));
1771  }
1772 
1773  VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypointList, maxDeviation),
1774  maxVelocities,
1775  maxAccelerations, timestepValue);
1776  ARMARX_CHECK_EXPRESSION(timeOptimalTraj.isValid());
1777 
1778 
1779  TrajectoryPtr newTraj = new Trajectory();
1780 
1781  Ice::DoubleSeq newTimestamps;
1782  double duration = timeOptimalTraj.getDuration();
1783  newTimestamps.reserve(duration / timestepValue + 1);
1784  for (double t = 0.0; t < duration; t += timestepValue)
1785  {
1786  newTimestamps.push_back(t);
1787  }
1788  newTimestamps.push_back(duration);
1789 
1790  for (size_t d = 0; d < dimensionNames.size(); d++)
1791  {
1792  Ice::DoubleSeq position;
1793  position.reserve(newTimestamps.size());
1794  for (double t = 0.0; t < duration; t += timestepValue)
1795  {
1796  position.push_back(timeOptimalTraj.getPosition(t)[d]);
1797  }
1798  position.push_back(timeOptimalTraj.getPosition(duration)[d]);
1799  newTraj->addDimension(position, newTimestamps, dimensionNames.at(d));
1800 
1801  Ice::DoubleSeq derivs;
1802  derivs.reserve(newTimestamps.size());
1803 
1804  for (double t = 0.0; t < duration; t += timestepValue)
1805  {
1806  derivs.clear();
1807  derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
1808  derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
1809  newTraj->addDerivationsToDimension(d, t, derivs);
1810  }
1811  derivs.clear();
1812  derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
1813  derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
1814  newTraj->addDerivationsToDimension(d, duration, derivs);
1815  }
1816  newTraj->setLimitless(limitless);
1817  if (hasLimitlessDimension)
1818  {
1820  }
1821  return newTraj;
1822  }
1823 
1824 
1825 
1826 
1827 
1828 
1829 
1830 
1831  size_t Trajectory::addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps, const std::string name)
1832  {
1833 
1834  const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
1835 
1836  size_t newDimIndex = dim();
1837 
1838  __addDimension();
1839 
1840  addPositionsToDimension(newDimIndex, values, tempTimestamps);
1841  if (newDimIndex < dimensionNames.size())
1842  {
1843  dimensionNames.at(newDimIndex) = name;
1844  }
1845  else
1846  {
1847  dimensionNames.push_back(name);
1848  }
1849  return newDimIndex;
1850  }
1851 
1852  void Trajectory::removeDimension(size_t dimension)
1853  {
1854  typename timestamp_view::iterator itMap = dataMap.begin();
1855 
1856  for (; itMap != dataMap.end(); itMap++)
1857  {
1858  std::vector< DoubleSeqPtr >& data = itMap->data;
1859 
1860  if (dimension < data.size())
1861  {
1862  data.erase(data.begin() + dimension);
1863  }
1864  }
1865  if (dimension < dimensionNames.size())
1866  {
1867  dimensionNames.erase(dimensionNames.begin() + dimension);
1868  }
1869  }
1870 
1871  void Trajectory::removeDerivation(size_t derivation)
1872  {
1873  typename timestamp_view::iterator itMap = dataMap.begin();
1874 
1875  for (; itMap != dataMap.end(); itMap++)
1876  {
1877  std::vector< DoubleSeqPtr >& data = itMap->data;
1878 
1879  for (auto& vec : data)
1880  {
1881  if (derivation + 1 < vec->size())
1882  {
1883  vec->resize(derivation);
1884  }
1885  }
1886  }
1887  }
1888 
1889  void Trajectory::removeDerivation(size_t dimension, size_t derivation)
1890  {
1891  typename timestamp_view::iterator itMap = dataMap.begin();
1892 
1893  for (; itMap != dataMap.end(); itMap++)
1894  {
1895  std::vector< DoubleSeqPtr >& data = itMap->data;
1896 
1897  if (data.size() > dimension && derivation + 1 < data.at(dimension)->size())
1898  {
1899  data.at(dimension)->resize(derivation);
1900  }
1901  }
1902  }
1903 
1904  Trajectory::ordered_view::const_iterator Trajectory::begin() const
1905  {
1906  return dataMap.get<TagOrdered>().begin();
1907  }
1908 
1909  Trajectory::ordered_view::const_iterator Trajectory::end() const
1910  {
1911  return dataMap.get<TagOrdered>().end();
1912  }
1913 
1914  Trajectory::ordered_view::const_reverse_iterator Trajectory::rbegin() const
1915  {
1916  return dataMap.get<TagOrdered>().rbegin();
1917  }
1918 
1919  Trajectory::ordered_view::const_reverse_iterator Trajectory::rend() const
1920  {
1921  return dataMap.get<TagOrdered>().rend();
1922  }
1923 
1924  std::vector<DoubleSeqPtr>& Trajectory::operator[](double timestamp)
1925  {
1926  return getStates(timestamp);
1927  }
1928 
1929 
1930 
1931  void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps)
1932  {
1933  if (dimension >= dim() && dim() > 0)
1934  {
1935  addDimension(values, timestamps);
1936  }
1937  else
1938  {
1939  ARMARX_CHECK_EXPRESSION(timestamps.size() == values.size()) << timestamps.size() << ", " << values.size();
1940 
1941  for (size_t i = 0; i < timestamps.size(); ++i)
1942  {
1943  checkValue(timestamps[i]);
1944  checkValue(values[i]);
1945  setPositionEntry(timestamps[i], dimension, values[i]);
1946  }
1947  }
1948  }
1949 
1950 
1951  void Trajectory::addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs)
1952  {
1953  setEntries(t, dimension, derivs);
1954  }
1955 
1957  {
1958  trajectory = traj;
1959  }
1960 
1962  {
1963  return data.at(dim);
1964  }
1965 
1967  {
1968  return timestamp;
1969  }
1970 
1972  {
1973  return getDeriv(dim, 0);
1974  }
1975 
1977  {
1978  if (!trajectory)
1979  {
1980  throw LocalException("Ptr to trajectory is NULL");
1981  }
1982  size_t numDim = trajectory->dim();
1983  Eigen::VectorXf result(numDim);
1984  for (std::size_t i = 0; i < numDim; ++i)
1985  {
1986  result(i) = getPosition(i);
1987  }
1988  return result;
1989  }
1990 
1992  {
1993  if (!trajectory)
1994  {
1995  throw LocalException("Ptr to trajectory is NULL");
1996  }
1997  size_t numDim = trajectory->dim();
1998  Eigen::VectorXd result(numDim);
1999  for (std::size_t i = 0; i < numDim; ++i)
2000  {
2001  result(i) = getPosition(i);
2002  }
2003  return result;
2004  }
2005 
2006  double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
2007  {
2008  if (!trajectory)
2009  {
2010  throw LocalException("Ptr to trajectory is NULL");
2011  }
2012  return trajectory->getState(timestamp, dim, derivation);
2013  }
2014 
2015  const std::vector<DoubleSeqPtr>& Trajectory::TrajData::getData() const
2016  {
2017  return data;
2018  }
2019 
2020 
2021 
2022 
2023  void Trajectory::shiftTime(double shift)
2024  {
2025  const ordered_view& ordv = dataMap.get<TagOrdered>();
2026  typename ordered_view::const_iterator itMap = ordv.begin();
2027  Trajectory shiftedTraj;
2028  CopyMetaData(*this, shiftedTraj);
2029  auto d = dim();
2030  for (; itMap != ordv.end(); itMap++)
2031  {
2032  for (size_t i = 0; i < d; ++i)
2033  {
2034  shiftedTraj.setEntries(itMap->timestamp + shift, i, *itMap->getData().at(i));
2035  }
2036  }
2037  // dataMap.swap(shiftedTraj.dataMap);
2038  *this = shiftedTraj;
2039  }
2040 
2041  void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
2042  {
2043  if (shift.size() > dim())
2044  {
2045  throw LocalException("dimension is out of range: ") << shift.size() << " actual dimensions: " << dim();
2046  }
2047 
2048 
2049  for (size_t dimension = 0; dimension < dim(); dimension++)
2050  {
2051  const ordered_view& ordv = dataMap.get<TagOrdered>();
2052  typename ordered_view::const_iterator itMap = ordv.begin();
2053 
2054  for (; itMap != ordv.end(); itMap++)
2055  {
2056  itMap->data[dimension]->at(0) += shift[dimension];
2057  }
2058  }
2059 
2060  }
2061 
2062  void Trajectory::scaleValue(const Ice::DoubleSeq& factor)
2063  {
2064  if (factor.size() > dim())
2065  {
2066  throw LocalException("dimension is out of range: ") << factor.size() << " actual dimensions: " << dim();
2067  }
2068 
2069 
2070  for (size_t dimension = 0; dimension < dim(); dimension++)
2071  {
2072  const ordered_view& ordv = dataMap.get<TagOrdered>();
2073  typename ordered_view::const_iterator itMap = ordv.begin();
2074 
2075  for (; itMap != ordv.end(); itMap++)
2076  {
2077  itMap->data[dimension]->at(0) *= factor[dimension];
2078  }
2079  }
2080 
2081  }
2082 
2083 
2084  void Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
2085  {
2086  limitless = limitlessStates;
2087  }
2088 
2089  LimitlessStateSeq Trajectory::getLimitless() const
2090  {
2091  return limitless;
2092  }
2093 
2094 
2095 } // namespace armarx
armarx::Trajectory::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Trajectory.cpp:131
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::math::MathUtils::ILerp
static float ILerp(float a, float b, float f)
Definition: MathUtils.h:160
armarx::Trajectory::TrajData::getTimestamp
double getTimestamp() const
Definition: Trajectory.cpp:1966
armarx::math::MathUtils::AngleLerp
static float AngleLerp(float a, float b, float f)
Definition: MathUtils.h:170
armarx::Trajectory::cloneMetaData
TrajectoryPtr cloneMetaData() const
Definition: Trajectory.cpp:136
ARMARX_CHECK_NOT_EQUAL
#define ARMARX_CHECK_NOT_EQUAL(lhs, rhs)
This macro evaluates whether lhs is inequal (!=) rhs and if it turns out to be false it will throw an...
Definition: ExpressionException.h:137
armarx::Trajectory::CopyMetaData
static void CopyMetaData(const Trajectory &source, Trajectory &destination)
Definition: Trajectory.cpp:1643
MathUtils.h
VectorHelpers.h
armarx::Trajectory::getMinimaTimestamps
Ice::DoubleSeq getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:726
armarx::Trajectory::getMin
double getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:659
armarx::Trajectory::gaussianFilter
void gaussianFilter(double filterRadius)
gaussianFilter smoothes the trajectory
Definition: Trajectory.cpp:1551
armarx::Trajectory::scaleValue
void scaleValue(const Ice::DoubleSeq &factor)
Definition: Trajectory.cpp:2062
armarx::Trajectory::TrajData::getPositionsAsVectorXd
Eigen::VectorXd getPositionsAsVectorXd() const
Definition: Trajectory.cpp:1991
armarx::Trajectory::getSquaredLength
double getSquaredLength(size_t dimension, size_t derivation) const
Definition: Trajectory.cpp:624
armarx::Trajectory::addDerivationsToDimension
void addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq &derivs)
Definition: Trajectory.cpp:1951
armarx::Trajectory::normalize
TrajectoryPtr normalize(const double startTime=0.0, const double endTime=1.0)
Definition: Trajectory.cpp:1733
armarx::Trajectory::getAllStates
std::vector< Ice::DoubleSeq > getAllStates(double t, int maxDeriv=1)
Definition: Trajectory.cpp:318
armarx::Trajectory::setLimitless
void setLimitless(const LimitlessStateSeq &limitlessStates)
Definition: Trajectory.cpp:2084
armarx::Trajectory::TrajData::TrajData
TrajData()
Definition: Trajectory.h:87
armarx::Trajectory::negateDim
void negateDim(size_t trajDimension)
negateDim changes the sign of all values of the given dimension.
Definition: Trajectory.cpp:1428
armarx::TrajectoryController::FoldLimitlessJointPositions
static void FoldLimitlessJointPositions(TrajectoryPtr traj)
Definition: TrajectoryController.cpp:161
armarx::Trajectory::clear
void clear(bool keepMetaData=false)
Definition: Trajectory.cpp:1651
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:35
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
armarx::Trajectory::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Trajectory.cpp:172
armarx::Trajectory::TrajData
Definition: Trajectory.h:85
armarx::Trajectory::data
TrajDataContainer & data()
Definition: Trajectory.cpp:364
armarx::Trajectory::TagOrdered
Definition: Trajectory.h:180
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::Trajectory::GenerateTimestamps
static Ice::DoubleSeq GenerateTimestamps(double startTime=0.0, double endTime=1.0, double stepSize=0.001)
Definition: Trajectory.cpp:1663
AbstractObjectSerializer.h
armarx::sign
T sign(T t)
Definition: algorithm.h:194
armarx::Trajectory::__fillAllEmptyFields
void __fillAllEmptyFields()
Definition: Trajectory.cpp:1083
armarx::Trajectory::getDiscreteDifferentiationForDim
Ice::DoubleSeq getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const
Definition: Trajectory.cpp:1136
armarx::Trajectory::getStatesAround
std::map< double, Ice::DoubleSeq > getStatesAround(double t, size_t derivation, size_t extend) const
Definition: Trajectory.cpp:946
armarx::Trajectory::getState
double getState(double t, size_t dim=0, size_t derivation=0)
Definition: Trajectory.cpp:225
armarx::Trajectory::rbegin
ordered_view::const_reverse_iterator rbegin() const
Definition: Trajectory.cpp:1914
armarx::Trajectory::getDiscretDifferentiationForDimAtT
double getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const
Definition: Trajectory.cpp:1228
armarx::Trajectory::addDimension
size_t addDimension(const Ice::DoubleSeq &values, const Ice::DoubleSeq &timestamps=Ice::DoubleSeq(), const std::string name="")
Definition: Trajectory.cpp:1831
armarx::Trajectory::end
ordered_view::const_iterator end() const
Definition: Trajectory.cpp:1909
IceUtil
Definition: Instance.h:21
armarx::Trajectory::operator[]
std::vector< DoubleSeqPtr > & operator[](double timestamp)
Definition: Trajectory.cpp:1924
armarx::Trajectory::~Trajectory
~Trajectory() override
armarx::Trajectory::getDimensionDataAsEigen
Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const
Definition: Trajectory.cpp:395
armarx::Trajectory::getLength
double getLength(size_t derivation=0) const
Returns the sum of a all subsequent distances of the entries in the trajectories over all dimensions.
Definition: Trajectory.cpp:524
armarx::Trajectory::rend
ordered_view::const_reverse_iterator rend() const
Definition: Trajectory.cpp:1919
armarx::Trajectory::calculateTimeOptimalTrajectory
TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, IceUtil::Time const &timestep)
Definition: Trajectory.cpp:1740
armarx::Trajectory::getType
Ice::Int getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Trajectory.cpp:162
armarx::Trajectory::ice_preMarshal
void ice_preMarshal() override
Definition: Trajectory.cpp:41
armarx::Trajectory::getPart
TrajectoryPtr getPart(double startTime, double endTime, size_t numberOfDerivations=0) const
Definition: Trajectory.cpp:462
armarx::Trajectory::dataMap
TrajDataContainer dataMap
Definition: Trajectory.h:440
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::Trajectory::getMax
double getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:654
armarx::Trajectory::shiftValue
void shiftValue(const Ice::DoubleSeq &shift)
Definition: Trajectory.cpp:2041
armarx::Trajectory::begin
ordered_view::const_iterator begin() const
Iterators that iterates in incremental order of the timestamps through the trajectory.
Definition: Trajectory.cpp:1904
armarx::Trajectory::removeDerivation
void removeDerivation(size_t derivation)
Definition: Trajectory.cpp:1871
armarx::Trajectory::reconstructFromDerivativeForDim
void reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
Definition: Trajectory.cpp:1392
armarx::Trajectory::__gaussianFilter
double __gaussianFilter(double filterRadius, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
Definition: Trajectory.cpp:1575
armarx::Trajectory::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: Trajectory.cpp:67
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::Trajectory::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Trajectory.cpp:143
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::Trajectory::differentiateDiscretly
void differentiateDiscretly(size_t derivation)
Definition: Trajectory.cpp:1371
armarx::TrajectoryController::UnfoldLimitlessJointPositions
static void UnfoldLimitlessJointPositions(TrajectoryPtr traj)
Definition: TrajectoryController.cpp:125
armarx::Trajectory::TrajData::getPositionsAsVectorXf
Eigen::VectorXf getPositionsAsVectorXf() const
Definition: Trajectory.cpp:1976
armarx::Trajectory::getMinima
Ice::DoubleSeq getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
Calculate all minima.
Definition: Trajectory.cpp:691
armarx::Trajectory::getDimensionName
std::string getDimensionName(size_t dim) const
Definition: Trajectory.cpp:354
armarx::Trajectory::setDimensionNames
void setDimensionNames(const Ice::StringSeq dimNames)
Definition: Trajectory.h:423
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::Trajectory::TrajData::operator[]
DoubleSeqPtr operator[](size_t dim) const
Definition: Trajectory.cpp:1961
armarx::checkValue
void checkValue(const T &value)
Definition: VectorHelpers.h:37
armarx::Trajectory::NormalizeTimestamps
static Trajectory NormalizeTimestamps(const Trajectory &traj, const double startTime=0.0, const double endTime=1.0)
Definition: Trajectory.cpp:1706
armarx::Trajectory::getDimensionNames
const Ice::StringSeq & getDimensionNames() const
Definition: Trajectory.cpp:359
armarx::Trajectory::setEntries
void setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq &y)
Definition: Trajectory.cpp:1025
armarx::Trajectory::__calcBaseDataAtTimestamp
std::vector< DoubleSeqPtr > __calcBaseDataAtTimestamp(const double &t) const
Definition: Trajectory.cpp:831
armarx::Trajectory::differentiateDiscretlyForDim
void differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
Definition: Trajectory.cpp:1380
armarx::Trajectory::getStates
std::vector< DoubleSeqPtr > & getStates(double t)
Definition: Trajectory.cpp:903
armarx::Trajectory::ordered_view
typename boost::multi_index::index< TrajDataContainer, TagOrdered >::type ordered_view
Definition: Trajectory.h:192
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::Trajectory::size
size_t size() const
Returns number of data entries (i.e.
Definition: Trajectory.cpp:503
armarx::Trajectory::getDimensionData
Ice::DoubleSeq getDimensionData(size_t dimension, size_t derivation=0) const
getDimensionData gets all entries for one dimensions with order of increasing timestamps
Definition: Trajectory.cpp:370
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
armarx::Trajectory::shiftTime
void shiftTime(double shift)
Definition: Trajectory.cpp:2023
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::Trajectory::removeDimension
void removeDimension(size_t dimension)
Definition: Trajectory.cpp:1852
armarx::VariantType::Trajectory
const VariantTypeId Trajectory
Definition: Trajectory.h:44
armarx::Trajectory::getMaxima
Ice::DoubleSeq getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:761
armarx::Trajectory::Trajectory
Trajectory()
Definition: Trajectory.h:196
armarx::Trajectory::addPositionsToDimension
void addPositionsToDimension(size_t dimension, const Ice::DoubleSeq &values, const Ice::DoubleSeq &timestamps)
Definition: Trajectory.cpp:1931
armarx::Trajectory::TrajData::getDeriv
double getDeriv(size_t dim, size_t derivation) const
Definition: Trajectory.cpp:2006
armarx::Trajectory::operator+=
Trajectory & operator+=(const Trajectory traj)
Definition: Trajectory.cpp:999
armarx::Trajectory::DifferentiateDiscretly
static Ice::DoubleSeq DifferentiateDiscretly(const Ice::DoubleSeq &timestamps, const Ice::DoubleSeq &values, int derivationCount=1)
Definition: Trajectory.cpp:1185
armarx::Trajectory::__fillBaseDataAtTimestamp
timestamp_view::iterator __fillBaseDataAtTimestamp(const double &t)
Definition: Trajectory.cpp:865
armarx::Trajectory::__addDimension
void __addDimension()
Definition: Trajectory.cpp:1013
armarx::Trajectory::toEigen
Eigen::MatrixXd toEigen(size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:431
armarx::Trajectory::getAmplitude
double getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:686
Trajectory.h
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::Trajectory::TrajData::timestamp
double timestamp
Definition: Trajectory.h:170
armarx::Trajectory::TrajDataContainer
boost::multi_index::multi_index_container< TrajData, boost::multi_index::indexed_by< boost::multi_index::hashed_unique< boost::multi_index::tag< TagTimestamp >, boost::multi_index::member< TrajData, double, &TrajData::timestamp > >, boost::multi_index::ordered_unique< boost::multi_index::tag< TagOrdered >, boost::multi_index::member< TrajData, double, &TrajData::timestamp > > > > TrajDataContainer
Definition: Trajectory.h:190
armarx::Trajectory::getTimestampsFloat
Ice::FloatSeq getTimestampsFloat() const
Definition: Trajectory.cpp:1118
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::Trajectory
The Trajectory class represents n-dimensional sampled trajectories.
Definition: Trajectory.h:76
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:916
armarx::Trajectory::TrajData::data
std::vector< DoubleSeqPtr > data
Definition: Trajectory.h:171
armarx::Trajectory::operator=
Trajectory & operator=(const Trajectory &source)
Definition: Trajectory.cpp:218
armarx::armem::server::ltm::detail::mixin::Path
std::filesystem::path Path
Definition: DiskStorageMixin.h:17
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
armarx::Trajectory::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Trajectory.cpp:167
Logging.h
armarx::Trajectory::dim
size_t dim() const
dim returns the trajectory dimension count for this trajectory (e.g.
Definition: Trajectory.cpp:491
armarx::Trajectory::serialize
void serialize(const ObjectSerializerBasePtr &obj, const Ice::Current &=Ice::emptyCurrent) const override
Definition: Trajectory.cpp:85
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::Trajectory::getTimeLength
double getTimeLength() const
Difference between biggest and smallest timestamp.
Definition: Trajectory.cpp:510
armarx::Trajectory::dataExists
bool dataExists(double t, size_t dimension=0, size_t derivation=0) const
Definition: Trajectory.cpp:850
armarx::Trajectory::getTimestamps
Ice::DoubleSeq getTimestamps() const
Definition: Trajectory.cpp:1103
armarx::Trajectory::getWithFunc
double getWithFunc(const double &(*foo)(const double &, const double &), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:664
TrajectoryController.h
armarx::Trajectory::CopyData
static void CopyData(const Trajectory &source, Trajectory &destination)
Definition: Trajectory.cpp:1624
armarx::math::MathUtils::Lerp
static float Lerp(float a, float b, float f)
Definition: MathUtils.h:150
armarx::Trajectory::getMaximaTimestamps
Ice::DoubleSeq getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
Definition: Trajectory.cpp:795
armarx::Trajectory::TrajData::getPosition
double getPosition(size_t dim) const
Definition: Trajectory.cpp:1971
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::Trajectory::__interpolate
double __interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const
Definition: Trajectory.cpp:1475
armarx::Trajectory::getDerivations
Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const
Definition: Trajectory.cpp:337
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::Trajectory::TrajData::trajectory
Trajectory * trajectory
Definition: Trajectory.h:173
armarx::DoubleSeqPtr
std::shared_ptr< Ice::DoubleSeq > DoubleSeqPtr
Definition: Trajectory.h:49
armarx::Trajectory::TrajData::getData
const std::vector< DoubleSeqPtr > & getData() const
Definition: Trajectory.cpp:2015
armarx::Trajectory::deserialize
void deserialize(const ObjectSerializerBasePtr &, const Ice::Current &=Ice::emptyCurrent) override
Definition: Trajectory.cpp:108
armarx::Trajectory::getLimitless
LimitlessStateSeq getLimitless() const
Definition: Trajectory.cpp:2089
armarx::Trajectory::setPositionEntry
void setPositionEntry(const double t, const size_t dimIndex, const double &y)
Definition: Trajectory.cpp:1055