Calculate nearest point to path taking into account the orientation [closed]
Hi all,
I will post here this question to see if you can help me, theoretically, on how to cope with this problem.
So, lets imagine I have a set of points stored in a vector representing the set of poses that generate a path that needs to be followed by a robot.
This points are defined by (x, y, z, yaw) vectors and the problem is to find, having the robot current pose (X, Y, Z, Yaw) the nearest pose (X', Y', Z', Yaw') in order to compute the correct control commands with that point.
Initially to solve this problem I used a KD-Tree, building it with every pose tuple of the path and then, in execution time, perform a search to find the nearest point with the current robot pose. However this solution seems to work only when the tuples are of 3 dimensions: (X, Y, Z,), when I tried to include the Yaw, the search tree does not work properly and freezes in the search with the same point until the robot is forced to move away from that computed pose.
I want to know if someone experience something like that, and if so, how did you solve it?
Furthermore I would like to know if there is a more complete/better approach for this particular task.
Thanks.
What does this have to do with ROS? Seems like a generic robotics question. Maybe something like the Robotics Stack Exchange would be a better place to ask this question: https://robotics.stackexchange.com/
Please update your question to clarify how this question involves ROS. Otherwise, we will close for being off-topic.
As an aside, when working with rotational space (e.g. the yaw you mention) and K-D trees, care must be taken to ensure the space partitioning is being handled correctly for these non-Euclidean spaces. In your case, the non-uniqueness of a given yaw orientation must be addressed in a K-D tree implementation. You mention nothing about how you are implementing your K-D tree, or what library you're using so it's tough to say more detail beyond what I've already said.
Perhaps you could review Fast Nearest Neighbor Search in SE(3) for Sampling-Based Motion Planning, WAFR 2014 for some more details on why this is challenging and a potential solution that could work for you.
Well, since I wass implementing this feature in a ROS node this is the first place I visit to post the answers, but, as stated by @jarvisschultz, this has nothing to do with ROS, thusI will close it,
Thanks for your answer and sorry for the inconvenience.
Regards.
No problem at all... best of luck asking elsewhere