Trajectory.cpp
Go to the documentation of this file.
1 #include "Trajectory.h"
2
3 #include <algorithm>
4 #include <cmath>
5 #include <cstddef>
6 #include <iterator>
7 #include <limits>
8
9 #include <Eigen/Core>
10 #include <Eigen/Geometry>
11
12 #include <SimoxUtility/shapes/CircleBase.h>
13 #include <VirtualRobot/MathTools.h>
14 #include <VirtualRobot/math/Helpers.h>
15 #include <VirtualRobot/math/LinearInterpolatedPose.h>
16
20
24
25 #include <range/v3/action/insert.hpp>
26 #include <range/v3/algorithm/none_of.hpp>
27 #include <range/v3/numeric/accumulate.hpp>
28 #include <range/v3/range/conversion.hpp>
29 #include <range/v3/view/all.hpp>
30 #include <range/v3/view/concat.hpp>
31 #include <range/v3/view/for_each.hpp>
32 #include <range/v3/view/group_by.hpp>
33 #include <range/v3/view/reverse.hpp>
34 #include <range/v3/view/sliding.hpp>
35 #include <range/v3/view/transform.hpp>
36 #include <range/v3/view/zip.hpp>
37
38 // FIXME move to simox
39
40 namespace simox
41 {
42  using Circlef = Circle<float>;
43
44  struct Segment2D
45  {
46  Eigen::Vector2f start;
47  Eigen::Vector2f end;
48
49  Eigen::Vector2f
50  d() const
51  {
52  return end - start;
53  };
54  };
55
56  // TODO only infinite line implemented. need to check line segment!
57  bool
58  intersects(const Circlef& circle, const Segment2D& segment)
59  {
60  // https://mathworld.wolfram.com/Circle-LineIntersection.html
61
62  const Segment2D localSegment{.start = segment.start - circle.center(),
63  .end = segment.end - circle.center()};
64
65  const float d_r = localSegment.d().norm();
66  const float D = localSegment.start.x() * localSegment.end.y() - localSegment.end.x() -
67  localSegment.start.y();
68
69  const auto delta =
70  static_cast<float>(std::pow(circle.radius(), 2) * std::pow(d_r, 2) - std::pow(D, 2));
71
72  if (delta < 0) // no intersection
73  {
74  return false;
75  }
76
77  if (delta == 0) // tangent
78  {
79  return true;
80  }
81
82  return true; // intersection
83  }
84
85  // TODO only infinite line implemented. need to check line segment!
86  std::vector<Eigen::Vector2f>
87  intersection(const Circlef& circle, const Segment2D& segment)
88  {
89
90  const Segment2D localSegment{.start = segment.start - circle.center(),
91  .end = segment.end - circle.center()};
92
93  const float d_r = localSegment.d().norm();
94  const float D = localSegment.start.x() * localSegment.end.y() - localSegment.end.x() -
95  localSegment.start.y();
96
97  const auto delta =
98  static_cast<float>(std::pow(circle.radius(), 2) * std::pow(d_r, 2) - std::pow(D, 2));
99
100  if (delta < 0) // no intersection
101  {
102  return std::vector<Eigen::Vector2f>{};
103  }
104
105  // Hint: + circle.center.x => local to global
106
107  if (delta == 0) // tangent
108  {
109  std::vector<Eigen::Vector2f> v;
110  v.emplace_back(D * localSegment.d().y() / std::pow(d_r, 2) + circle.center().x(),
111  -D * localSegment.d().x() / std::pow(d_r, 2) + circle.center().y());
112  return v;
113  }
114
115  // intersection
116
117  const float hx =
118  std::copysign(localSegment.d().y(), localSegment.d().x()) * std::sqrt(delta);
119  const float hy = std::fabs(localSegment.d().y()) * std::sqrt(delta);
120
121  std::vector<Eigen::Vector2f> v;
122  v.emplace_back((D * localSegment.d().y() + hx) / std::pow(d_r, 2) + circle.center().x(),
123  (-D * localSegment.d().x() + hy) / std::pow(d_r, 2) + circle.center().y());
124  v.emplace_back((D * localSegment.d().y() - hx) / std::pow(d_r, 2) + circle.center().x(),
125  (-D * localSegment.d().x() - hy) / std::pow(d_r, 2) + circle.center().y());
126  return v;
127  }
128
129 } // namespace simox
130
131 // end fixme
132
134 {
135
136  namespace conv
137  {
138
140  toTrajectoryPoint(const Pose& pose, const float velocity)
141  {
142  return GlobalTrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
143  }
144
146  toTrajectoryPoints(const Path& path, const float velocity)
147  {
148  GlobalTrajectoryPoints trajectoryPoints;
149  trajectoryPoints.reserve(path.size());
150
151  std::transform(path.begin(),
152  path.end(),
153  std::back_inserter(trajectoryPoints),
155
156  return trajectoryPoints;
157  }
158
160  toTrajectoryPoints(const Path& path, const std::vector<float>& velocities)
161  {
162  ARMARX_CHECK_EQUAL(path.size(), velocities.size());
163
164  GlobalTrajectoryPoints trajectoryPoints;
165  trajectoryPoints.reserve(path.size());
166
167  const auto convert = [](const auto& p)
168  {
169  const auto& [pose, velocity] = p;
171  };
172
173  return ranges::views::zip(path, velocities) | ranges::views::transform(convert) |
174  ranges::to_vector;
175  }
176
177  } // namespace conv
178
179  GlobalTrajectory::InternalProjection
180  GlobalTrajectory::projectInternal(const Position& point,
181  const VelocityInterpolation& velocityInterpolation) const
182  {
184
185  InternalProjection bestProj;
186
187  for (size_t i = 0; i < pts.size() - 1; i++)
188  {
189  const auto& wpBefore = pts.at(i);
190  const auto& wpAfter = pts.at(i + 1);
191
192  /*
193  // FIXME remove after finding the bug
194  if ((wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
195  .norm() < 1.F)
196  {
197  // ARMARX_WARNING << "Trajectory segment " << i << " too small ...";
198  continue;
199  }
200  */
201
202  const auto closestPoint = VirtualRobot::MathTools::nearestPointOnSegment<Position>(
203  wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point);
204
205  const float currentDistance = (closestPoint - point).norm();
206
207  // 'less equal' to accept following segment if the closest point is the waypoint
208  if (currentDistance <= distance)
209  {
210  const float d1 = (closestPoint - wpBefore.waypoint.pose.translation()).norm();
211  const float d =
212  (wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
213  .norm();
214
215  // fraction of distance between segment end points
216  const float t = d1 / d;
217
218  math::LinearInterpolatedPose ip(
219  wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
220
221  bestProj.indexBefore = i;
222  bestProj.projection.waypoint.pose = ip.Get(t);
223
224  switch (velocityInterpolation)
225  {
227  bestProj.projection.velocity =
228  wpBefore.velocity + (wpAfter.velocity - wpBefore.velocity) * t;
229  break;
231  bestProj.projection.velocity = wpBefore.velocity;
232  break;
233  }
234
235  distance = currentDistance;
236  }
237  }
238
239  return bestProj;
240  }
241
242  Projection
244  const VelocityInterpolation& velocityInterpolation) const
245  {
247
248  if (pts.size() == 1)
249  {
250  return Projection{.projection = pts.front(),
251  .wayPointBefore = pts.front(),
252  .wayPointAfter = pts.front(),
253  .segment = Projection::Segment::FINAL};
254  }
255
256  InternalProjection intProj = projectInternal(point, velocityInterpolation);
257
258  const auto& wpBefore = pts.at(intProj.indexBefore);
259  const auto& wpAfter = pts.at(intProj.indexBefore + 1);
260
261  Projection bestProj;
262
263  bestProj.wayPointBefore = wpBefore;
264  bestProj.wayPointAfter = wpAfter;
265  bestProj.projection = intProj.projection;
266
267  bestProj.segment = [&]
268  {
269  // when segment is both first and final, prefer final
270  if (intProj.indexBefore == (pts.size() - 2))
271  {
273  }
274
275  if (intProj.indexBefore == 0)
276  {
278  }
279
281  }();
282
283  return bestProj;
284  }
285
286  std::pair<GlobalTrajectory, bool>
288  {
289  const InternalProjection intProj =
290  projectInternal(point, VelocityInterpolation::LinearInterpolation);
291
292  GlobalTrajectory subTraj{};
293  subTraj.mutablePoints().push_back(intProj.projection);
294
295  float remainingDistance = distance;
296  GlobalTrajectoryPoint lastWp = intProj.projection;
297  for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
298  {
299  const auto& nextWp = pts.at(i);
300
301  const float segmentDistance =
302  (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
303
304  if (segmentDistance < remainingDistance)
305  {
306  subTraj.mutablePoints().push_back(nextWp);
307  lastWp = nextWp;
308  remainingDistance -= segmentDistance;
309  }
310  else
311  {
312  // fraction of distance between segment end points
313  const float t = remainingDistance / segmentDistance;
314
315  math::LinearInterpolatedPose ip(
316  lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
317  const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
318
319  subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
320
321  return {subTraj, false};
322  }
323  }
324
325  // remaining trajectory is shorter than the specified distance
326  return {subTraj, true};
327  }
328
329  std::vector<Eigen::Vector3f>
331  {
332  const auto toPosition = [](const GlobalTrajectoryPoint& wp)
333  { return wp.waypoint.pose.translation(); };
334
335  std::vector<Eigen::Vector3f> positions;
336  positions.reserve(pts.size());
337
338  std::transform(pts.begin(), pts.end(), std::back_inserter(positions), toPosition);
339
340  return positions;
341  }
342
343  std::vector<Pose>
344  GlobalTrajectory::poses() const noexcept
345  {
346  std::vector<Pose> poses;
347  poses.reserve(pts.size());
348
349  std::transform(pts.begin(),
350  pts.end(),
351  std::back_inserter(poses),
352  [](const core::GlobalTrajectoryPoint& pt) -> core::Pose
353  { return pt.waypoint.pose; });
354
355  return poses;
356  }
357
359  GlobalTrajectory::FromPath(const Path& path, const float velocity)
360  {
361  return {conv::toTrajectoryPoints(path, velocity)};
362  }
363
365  GlobalTrajectory::FromPath(const Path& path, const std::vector<float>& velocity)
366  {
367  return conv::toTrajectoryPoints(path, velocity);
368  }
369
372  const Positions& waypoints,
373  const Pose& goal,
374  const float velocity)
375  {
376  // currently, only 2D version
377
378  Path path;
379  path.reserve(waypoints.size());
380
381  if (waypoints.empty())
382  {
383  return FromPath({start, goal}, velocity);
384  }
385
386  path.push_back(start);
387
388  const auto directedPose = [](const Position& position, const Position& target) -> Pose
389  {
390  const Direction direction = target - position;
391
392  const float yaw = math::Helpers::Angle(navigation::conv::to2D(direction));
393
394  // our robot is moving into y direction ( y is forward ... )
395  const Eigen::Rotation2Df pose_R_robot =
396  Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
397
399  pose_R_robot);
400  };
401
402  // insert waypoints pointing to next waypoint
403  // for (size_t i = 0; i < waypoints.size() - 1; i++)
404  // {
405  // path.emplace_back(directedPose(waypoints.at(i), waypoints.at(i + 1)));
406  // }
407  ranges::insert(path,
408  path.end(),
409  waypoints | ranges::views::sliding(2) |
411  [&directedPose](const auto& p) -> Pose
412  {
413  const auto& [position, _] = p;
414  const auto target = position + 1;
415
416  return directedPose(*position, *target);
417  }));
418
419  path.emplace_back(directedPose(waypoints.back(), goal.translation()));
420  path.push_back(goal);
421
422  return FromPath(path, velocity);
423  }
424
427  const Positions& waypoints,
428  const Pose& goal,
429  const std::vector<float>& velocity)
430  {
431
432  // currently, only 2D version
433
434  Path path;
435  path.reserve(waypoints.size());
436
437  if (waypoints.empty())
438  {
439  return FromPath({start, goal}, velocity);
440  }
441
442  path.push_back(start);
443
444  const auto directedPose = [](const Position& position, const Position& target) -> Pose
445  {
446  const Direction direction = target - position;
447
448  const float yaw = math::Helpers::Angle(navigation::conv::to2D(direction));
449
450  // our robot is moving into y direction ( y is forward ... )
451  const Eigen::Rotation2Df pose_R_robot =
452  Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
453
455  pose_R_robot);
456  };
457
458  // insert waypoints pointing to next waypoint
459  // for (size_t i = 0; i < waypoints.size() - 1; i++)
460  // {
461  // path.emplace_back(directedPose(waypoints.at(i), waypoints.at(i + 1)));
462  // }
463  ranges::insert(path,
464  path.end(),
465  waypoints | ranges::views::sliding(2) |
467  [&directedPose](const auto& p) -> Pose
468  {
469  const auto& [position, _] = p;
470  const auto target = position + 1;
471
472  return directedPose(*position, *target);
473  }));
474
475  path.emplace_back(directedPose(waypoints.back(), goal.translation()));
476  path.push_back(goal);
477
478  return FromPath(path, velocity);
479  }
480
482  resamplePath(const auto& pts, const float eps)
483  {
484  core::Positions resampledPath;
485
486  const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
487  { return wp.waypoint.pose; };
488
489  const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
490
491  // resampledPath.push_back(originalPath.front().translation());
492
493  // the current / last added point to the resampled path
494  Position pointOnPath = originalPath.front().translation();
495
496  for (const auto& wp : originalPath | ranges::views::sliding(2))
497  {
498  const auto& [wp1, _] = wp;
499  const auto wp2 = wp1 + 1;
500
501  ARMARX_TRACE;
502
503  ARMARX_DEBUG << "sliding ...";
504  ARMARX_DEBUG << "End: " << navigation::conv::to2D(wp2->translation());
505
506  const simox::Segment2D lineSegment{.start = navigation::conv::to2D(wp1->translation()),
508
509  // easy to add points along line
510  while ((navigation::conv::to2D(pointOnPath) - lineSegment.end).norm() >= eps)
511  {
512  const auto dir =
514
515  pointOnPath += dir * eps;
516  resampledPath.push_back(pointOnPath);
517  }
518
519  // find the segment that is sufficiently far away from the current pointOnPath
520  // skip those segments that are too close
521  // => find the first segment that penetrates the epsilon-region
522  const float distanceEnd =
524  if (distanceEnd < eps)
525  {
526  continue;
527  }
528
529  // add the intersection between the circle defining the epsilon region and the
530  // line segment
531
532  ARMARX_TRACE;
533  const auto isect =
535
536  if (not(isect.size() == 2))
537  {
538  ARMARX_WARNING << "intersection failure";
539  continue;
540  }
541
542  for (const auto& pt : isect)
543  {
544  ARMARX_DEBUG << "Intersection " << VAROUT(pt) << " distance "
546  }
547
548  // true intersection hypotheses
549  const auto d0 = (isect.at(0) - lineSegment.start).normalized();
550  // const auto d1 = isect.at(1) - lineSegment.start;
551
552  const auto d = (lineSegment.end - lineSegment.start).normalized();
553
554  // find out which point lies between start and end
555  // here, we check if the point is into end's direction from start (cosine similarity)
556  if (d0.dot(d) > 0)
557  {
559  }
560  else
561  {
563  }
564
565  resampledPath.push_back(pointOnPath);
566  }
567
568  // if (resampledPath.size() >= 2)
569  // {
570  // // remove first "waypoint" => this is the start
571  // resampledPath.erase(resampledPath.begin());
572
573  // // remove last "waypoint" => this is the end
574  // resampledPath.pop_back();
575  // }
576
577  return resampledPath;
578  }
579
580  GlobalTrajectory
581  GlobalTrajectory::resample(const float eps) const
582  {
583  // can only resample if at least a line segment is available
584  if (pts.size() < 2)
585  {
586  return *this;
587  }
588
589  // ARMARX_CHECK_GREATER_EQUAL(pts.size(), 2);
590
591  const core::Positions resampledPathForward = resamplePath(pts, eps);
592  const core::Positions resampledPathBackward =
594
595  if (resampledPathForward.empty() or resampledPathBackward.empty())
596  {
597  ARMARX_DEBUG << "The resampled path contains no points. This means that it is likely "
598  "very short.";
599
601  pts.front().waypoint.pose, resampledPathForward, pts.back().waypoint.pose, 0.F);
602
603  ARMARX_CHECK_LESS_EQUAL(testTrajectory.length(), eps)
604  << "The resampled trajectory is only allowed to contains no points if it is "
605  "shorter than eps";
606
607  return GlobalTrajectory({pts.front(), pts.back()});
608  }
609
610
611  ARMARX_DEBUG << "Resampled path forward contains " << resampledPathForward.size()
612  << " waypoints";
613  ARMARX_DEBUG << "Resampled path backwards contains " << resampledPathBackward.size()
614  << " waypoints";
615
616  core::Position pivot = resampledPathBackward.front();
617
618  core::Positions segmentForward;
619  core::Positions segmentBackward;
620
621  // Find the first two elements that are close enough to each other.
622  // The loop works as follows:
623  // - loop over the resampled paths
624  // - if an element is far enough away from the pivot element (the last element inserted into one of the segments),
625  // add it to the corresponding list
626  // - if the new element is close enough to the pivot element, we are done => solution found
627  for (const auto& [wpForward, wpBackward] :
628  ranges::views::zip(resampledPathForward, resampledPathBackward))
629  {
630  segmentForward.push_back(wpForward);
631  if ((pivot - wpForward).norm() < eps) // term. cond?
632  {
633  break;
634  }
635  pivot = wpForward;
636
637  segmentBackward.push_back(wpBackward);
638  if ((pivot - wpBackward).norm() < eps) // term. cond?
639  {
640  break;
641  }
642  pivot = wpBackward;
643  }
644
645  core::Positions resampledPath =
646  ranges::views::concat(segmentForward, segmentBackward | ranges::views::reverse) |
647  ranges::to_vector;
648
649
650  auto resampledTrajectory = GlobalTrajectory::FromPath(
651  pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
652
653  // set velocity based on original trajectory
654  {
655  const auto setVelocityInPlace = [&](GlobalTrajectoryPoint& pt)
656  {
657  const auto projection = getProjection(pt.waypoint.pose.translation(),
659  pt.velocity = projection.projection.velocity;
660  };
661
662  std::for_each(resampledTrajectory.mutablePoints().begin(),
663  resampledTrajectory.mutablePoints().end(),
664  setVelocityInPlace);
665  }
666
667
668  // sanity check: make sure that resampling was successful
669  // number of samples required for source trajectory
670  // const float minRequiredSamples = length() / eps;
671  // const size_t samples = resampledTrajectory.points().size();
672
673  // ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
674
675  // this is a rough approximation of the upper bound
676  // ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
677
678  return resampledTrajectory;
679  }
680
681  float
683  {
684  namespace r = ::ranges;
685
686  const auto distanceBetweenWaypoints = [](const auto& p) -> float
687  {
688  const auto& [p1, _] = p;
689  const auto p2 = p1 + 1;
690  return (p1->waypoint.pose.translation() - p2->waypoint.pose.translation()).norm();
691  };
692
693  auto rng = pts | r::views::sliding(2) | r::views::transform(distanceBetweenWaypoints);
694
695  const float length = r::accumulate(rng, 0.F);
696
698
699  return length;
700  }
701
702  void
704  {
705  ARMARX_CHECK_GREATER_EQUAL(maxVelocity, 0.F) << "maxVelocity must be positive!";
706
707  std::for_each(pts.begin(),
708  pts.end(),
709  [&maxVelocity](GlobalTrajectoryPoint& pt)
710  { pt.velocity = std::clamp(pt.velocity, -maxVelocity, maxVelocity); });
711  }
712
713  bool
715  {
716  namespace r = ::ranges;
717
718  const auto distanceBetweenWaypoints = [](const auto& p) -> float
719  {
720  const auto& [p1, _] = p;
721  const auto p2 = p1 + 1;
722  return (p1->waypoint.pose.translation() - p2->waypoint.pose.translation()).norm();
723  };
724
725  auto rng = pts | r::views::sliding(2) | r::views::transform(distanceBetweenWaypoints);
726
727  return ranges::none_of(
728  rng, [&maxDistance](const auto& dist) -> bool { return dist > maxDistance; });
729  }
730
731  const std::vector<GlobalTrajectoryPoint>&
733  {
734  return pts;
735  }
736
737  std::vector<GlobalTrajectoryPoint>&
739  {
740  return pts;
741  }
742
743  float
745  {
746
747  float dur = 0;
748
749  for (int i = 0; i < static_cast<int>(pts.size() - 1); i++)
750  {
751  const auto& start = pts.at(i);
752  const auto& goal = pts.at(i + 1);
753
754  const float distance =
755  (goal.waypoint.pose.translation() - start.waypoint.pose.translation()).norm();
756
757  const float startVelocity = start.velocity;
758  const float goalVelocity = goal.velocity;
759
760  constexpr int nSamples = 100;
761
762  for (int j = 0; j < nSamples; j++)
763  {
764  const float progress = static_cast<float>(j) / nSamples;
765
766  const float v = startVelocity + progress * (goalVelocity - startVelocity);
767  const float s = distance / nSamples;
768
769  const float t = s / v;
770
771  dur += t;
772  }
773  }
774
775  return dur;
776  }
777
778  bool
779  GlobalTrajectory::isValid() const noexcept
780  {
781  const auto isValid = [](const GlobalTrajectoryPoint& pt) -> bool
782  { return std::isfinite(pt.velocity) and pt.velocity < 3000; };
783
784  return std::all_of(pts.begin(), pts.end(), isValid);
785  }
786
788  GlobalTrajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
789  {
790  const core::Position referencePosition = pts.at(idx).waypoint.pose.translation();
791
792  const auto isInRange = [&](const std::size_t i) -> bool
793  {
794  const float posDiff =
795  (pts.at(i).waypoint.pose.translation() - referencePosition).norm();
796  // ARMARX_INFO << VAROUT(posDiff);
798  };
799
801
802
803  // traverse from query point to start
804  for (int i = static_cast<int>(idx) - 1; i >= 0; i--)
805  {
806  if (isInRange(i))
807  {
808  indices.push_back(i);
809  }
810  else
811  {
812  // the first one outside the radius is the termination condition
813  break;
814  }
815  }
816
817  // traverse from query point to end
818  for (int i = idx + 1; i < static_cast<int>(pts.size()); i++)
819  {
820  if (isInRange(i))
821  {
822  indices.push_back(i);
823  }
824  else
825  {
826  // the first one outside the radius is the termination condition
827  break;
828  }
829  }
830
831  ARMARX_DEBUG << indices.size() << " points in range";
832
833  return indices;
834  }
835
836  const std::vector<LocalTrajectoryPoint>&
838  {
839  return pts;
840  }
841
842  std::vector<LocalTrajectoryPoint>&
844  {
845  return pts;
846  }
847
848  Evaluation
850  {
851  const auto& finalTimestamp = points().back().timestamp;
852  const auto timestamp =
853  ts > finalTimestamp ? finalTimestamp - Duration::MilliSeconds(1) : ts;
854
855  const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
856  const DateTime& timestamp) -> bool
857  { return lhs.timestamp < timestamp; };
858
859  const auto lower = std::lower_bound(pts.begin(), pts.end(), timestamp, cmp);
860
861  if (lower == pts.end() - 1)
862  {
863  // if we arrived at the last point, the velocity is 0
864  return {lower->pose, core::Twist::Zero()};
865  }
866
867  const Duration d1 = timestamp - lower->timestamp;
868  const Duration dT = lower[1].timestamp - lower->timestamp;
869
870  // the waypoint before this timestamp
871  const auto& global_T_wp_before = lower->pose;
872
873  // the waypoint after this timestamp
874  const auto& global_T_wp_after = lower[1].pose;
875
876  // fraction of time between segment end points
877  const float t = d1 / dT;
878  math::LinearInterpolatedPose ip(
879  global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
880
881  const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
882
883  const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
884
885  // the movement direction in the global frame
886  const Eigen::Vector3f global_V_linear_movement_direction =
887  global_R_wp_before * wp_before_T_wp_after.translation().normalized();
888  const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
889  const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
890
891
892  // angular velocity
893
894  Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
895
896  const core::Twist feedforwardTwist{
897  .linear = linearVelocity * global_V_linear_movement_direction,
898  .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
899
900  return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
901  }
902
903
simox::intersects
bool intersects(const Circlef &circle, const Segment2D &segment)
Definition: Trajectory.cpp:58
VelocityInterpolation
Definition: Trajectory.h:60
std::vector< GlobalTrajectoryPoint > & mutablePoints()
Definition: Trajectory.cpp:738
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
Pose pose
Definition: basic_types.h:50
simox::Segment2D
Definition: Trajectory.cpp:44
std::vector< LocalTrajectoryPoint > & mutablePoints()
Definition: Trajectory.cpp:843
GlobalTrajectoryPoint wayPointAfter
Definition: Trajectory.h:48
armarx::core::time::Duration::toSecondsDouble
double toSecondsDouble() const
Returns the amount of seconds.
Definition: Duration.cpp:104
bool isValid() const noexcept
Definition: Trajectory.cpp:779
@ LinearInterpolation
LocalException.h
std::vector< std::size_t > Indices
Definition: Trajectory.h:126
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Waypoint waypoint
Definition: Trajectory.h:37
basic_types.h
static Twist Zero()
Definition: basic_types.cpp:13
std::vector< core::Pose > convert(const std::vector< Eigen::Matrix4f > &wps)
Definition: Component.cpp:101
float velocity
Definition: Trajectory.h:38
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
DateTime timestamp
Definition: Trajectory.h:154
GlobalTrajectoryPoints toTrajectoryPoints(const Path &path, const float velocity)
Definition: Trajectory.cpp:146
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
simox::intersection
std::vector< Eigen::Vector2f > intersection(const Circlef &circle, const Segment2D &segment)
Definition: Trajectory.cpp:87
GlobalTrajectory()=default
ARMARX_CHECK_NOT_EMPTY
#define ARMARX_CHECK_NOT_EMPTY(c)
Definition: ExpressionException.h:224
simox::Segment2D::d
Eigen::Vector2f d() const
Definition: Trajectory.cpp:50
std::vector< GlobalTrajectoryPoint > GlobalTrajectoryPoints
Definition: Trajectory.h:41
GlobalTrajectoryPoint wayPointBefore
Definition: Trajectory.h:47
const std::vector< LocalTrajectoryPoint > & points() const
Definition: Trajectory.cpp:837
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
void setMaxVelocity(float maxVelocity)
Definition: Trajectory.cpp:703
Definition: basic_types.h:53
armarx::reverse
T reverse(const T &o)
Definition: container.h:32
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:327
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
std::pair< GlobalTrajectory, bool > getSubTrajectory(const Position &point, const float distance) const
Project point onto the trajectory and return a new trajectory along the old one from that point for t...
Definition: Trajectory.cpp:287
GlobalTrajectoryPoint projection
Definition: Trajectory.h:45
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:737
float length() const
Definition: Trajectory.cpp:682
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
static GlobalTrajectory FromPath(const Path &path, float velocity)
Note: the velocity will not be set!
Definition: Trajectory.cpp:359
simox::Segment2D::end
Eigen::Vector2f end
Definition: Trajectory.cpp:47
std::vector< Pose > poses() const noexcept
Definition: Trajectory.cpp:344
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::armem::server::ltm::util::mongodb::detail::insert
bool insert(mongocxx::collection &coll, const nlohmann::json &value)
Definition: mongodb.cpp:43
simox::Circlef
Circle< float > Circlef
Definition: Trajectory.cpp:42
core::Positions resamplePath(const auto &pts, const float eps)
Definition: Trajectory.cpp:482
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
ARMARX_CHECK_LESS_EQUAL
#define ARMARX_CHECK_LESS_EQUAL(lhs, rhs)
This macro evaluates whether lhs is less or equal (<=) rhs and if it turns out to be false it will th...
Definition: ExpressionException.h:109
std::vector< Eigen::Vector2f > to2D(const std::vector< Eigen::Vector3f > &v)
Definition: eigen.cpp:26
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
LinearVelocity linear
Definition: basic_types.h:55
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
float duration(core::VelocityInterpolation interpolation) const
Definition: Trajectory.cpp:744
std::vector< Position > Positions
Definition: basic_types.h:37
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
std::vector< Pose > Path
Definition: basic_types.h:46
Evaluation evaluate(const DateTime &timestamp) const
Definition: Trajectory.cpp:849
Eigen::Vector3f Direction
Definition: basic_types.h:39
armarx::core::time::Duration
Represents a duration.
Definition: Duration.h:17
F
Definition: ExportDialogControllerTest.cpp:16
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:11
std::vector< Position > positions() const noexcept
Definition: Trajectory.cpp:330
Indices allConnectedPointsInRange(std::size_t idx, float radius) const
get all points within a certain radius that are directly connected to idx
Definition: Trajectory.cpp:788
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
simox::Segment2D::start
Eigen::Vector2f start
Definition: Trajectory.cpp:46
GlobalTrajectoryPoint toTrajectoryPoint(const Pose &pose, const float velocity)
Definition: Trajectory.cpp:140
Projection getProjection(const Position &point, const VelocityInterpolation &velocityInterpolation) const
Definition: Trajectory.cpp:243
eigen.h
Logging.h
Eigen::Vector3f Position
Definition: basic_types.h:36
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
Trajectory.h
bool hasMaxDistanceBetweenWaypoints(float maxDistance) const
Definition: Trajectory.cpp:714
point_type toPoint(const Eigen::Vector2f &pt)
Definition: geometry.cpp:42
simox
Definition: Impl.cpp:40
Segment segment
Definition: Trajectory.h:57
const std::vector< GlobalTrajectoryPoint > & points() const
Definition: Trajectory.cpp:732
types.h