CircularPathSmoothing.cpp
Go to the documentation of this file.
2 
3 #include <type_traits>
4 
5 #include <VirtualRobot/MathTools.h>
6 
9 
10 namespace
11 {
12  template <typename T>
13  constexpr T
14  deg2rad(T deg)
15  {
16  static_assert(std::is_floating_point<T>::value);
17  constexpr T c = M_PI / 180;
18  return deg * c;
19  }
20 } // namespace
21 
23 {
24  // TODO move to Simox
25  float
26  getAngleBetweenVectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2)
27  {
28  // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
29  const Eigen::Vector2f v1_vec_normed = v1.normalized();
30  const Eigen::Vector2f v2_vec_normed = v2.normalized();
31  const float dot_product_vec =
32  v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
33  float yaw = acos(dot_product_vec);
34  return yaw;
35  }
36 
39  {
40  Points points;
41 
42  points.push_back(p.front());
43 
44  // omit start and end
45  for (size_t i = 1; i < (p.size() - 1); i++)
46  {
47  const Eigen::Vector2f& prev = p.at(i - 1);
48  const Eigen::Vector2f& at = p.at(i);
49  const Eigen::Vector2f& next = p.at(i + 1);
50 
51  // the largest radius for smoothing
52  const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
53  const float radius = std::min(maxRadius, 300.F);
54 
55  const Eigen::Vector2f vIn = (prev - at).normalized();
56  const Eigen::Vector2f vOut = (next - at).normalized();
57 
58  // the angle between the lines
59  const float phi = getAngleBetweenVectors(vIn, vOut);
60  const float l = radius / std::tan(phi / 2);
61 
62  points.push_back(at + Eigen::Vector2f(prev - at).normalized() * l);
63  points.push_back(at + Eigen::Vector2f(next - at).normalized() * l);
64  }
65 
66  points.push_back(p.back());
67 
68  return points;
69  }
70 
73  {
75  const auto& p = trajectory.points();
76 
77  points.push_back(p.front());
78 
79  // omit start and end
80  for (size_t i = 1; i < (p.size() - 1); i++)
81  {
82  const Eigen::Vector2f& prev =
83  navigation::conv::to2D(p.at(i - 1).waypoint.pose.translation());
84  const Eigen::Vector2f& at = navigation::conv::to2D(p.at(i).waypoint.pose.translation());
85  const Eigen::Vector2f& next =
86  navigation::conv::to2D(p.at(i + 1).waypoint.pose.translation());
87 
88  // the largest radius for smoothing
89  const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
90  const float radius = std::min(maxRadius, 300.F);
91 
92  const Eigen::Vector2f vIn = (prev - at).normalized();
93  const Eigen::Vector2f vOut = (next - at).normalized();
94 
95  // the angle between the lines
96  const float phi = getAngleBetweenVectors(vIn, vOut);
97 
98  // decide whether it is necessary to smooth the path
99  if (std::abs(phi) < deg2rad(10.F))
100  {
101  points.push_back(p.at(i));
102  }
103  else
104  {
105  const float l = radius / std::tan(phi / 2);
106 
107  const Eigen::Vector2f prePos = at + Eigen::Vector2f(prev - at).normalized() * l;
108  const Eigen::Vector2f postPos = at + Eigen::Vector2f(next - at).normalized() * l;
109 
110  const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::GlobalTrajectoryPoint
111  {
112  const auto proj =
113  trajectory.getProjection(navigation::conv::to3D(pos),
115 
116  return proj.projection;
117  };
118 
119  points.push_back(tpProj(prePos));
120  points.push_back(tpProj(postPos));
121  }
122  }
123 
124  points.push_back(p.back());
125 
126  return points;
127  }
128 } // namespace armarx::navigation::algorithm
armarx::navigation::core::GlobalTrajectory
Definition: Trajectory.h:68
armarx::navigation::core::VelocityInterpolation::LinearInterpolation
@ LinearInterpolation
armarx::navigation::algorithm::getAngleBetweenVectors
float getAngleBetweenVectors(const Eigen::Vector2f &v1, const Eigen::Vector2f &v2)
Definition: CircularPathSmoothing.cpp:26
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::navigation::core::GlobalTrajectoryPoints
std::vector< GlobalTrajectoryPoint > GlobalTrajectoryPoints
Definition: Trajectory.h:41
armarx::navigation::core::GlobalTrajectoryPoint
Definition: Trajectory.h:35
CircularPathSmoothing.h
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::navigation::algorithm::CircularPathSmoothing::Points
std::vector< Point > Points
Definition: CircularPathSmoothing.h:39
armarx::navigation::core::Projection::projection
GlobalTrajectoryPoint projection
Definition: Trajectory.h:45
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::navigation::algorithm
This file is part of ArmarX.
Definition: AStarPlanner.cpp:29
armarx::navigation::algorithm::CircularPathSmoothing::smooth
Points smooth(const Points &p)
circular path smoothing
Definition: CircularPathSmoothing.cpp:38
armarx::navigation::conv::to2D
std::vector< Eigen::Vector2f > to2D(const std::vector< Eigen::Vector3f > &v)
Definition: eigen.cpp:26
F
Definition: ExportDialogControllerTest.cpp:16
armarx::navigation::conv::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:11
armarx::navigation::core::GlobalTrajectory::getProjection
Projection getProjection(const Position &point, const VelocityInterpolation &velocityInterpolation) const
Definition: Trajectory.cpp:243
eigen.h
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
min
T min(T t1, T t2)
Definition: gdiam.h:42
Trajectory.h
armarx::navigation::core::GlobalTrajectory::points
const std::vector< GlobalTrajectoryPoint > & points() const
Definition: Trajectory.cpp:732
norm
double norm(const Point &a)
Definition: point.hpp:94