Geometry Boost List: Re: [geometry] R-tree query nearest nodes with torus distance strategy

Subject: Re: [geometry] R-tree query nearest nodes with torus distance strategy
From: Adam Wulkiewicz (adam.wulkiewicz_at_[hidden])
Date: 2015-04-02 14:15:43

Andrew Hundt wrote:
> On Thu, Apr 2, 2015 at 9:43 AM, Adam Wulkiewicz 
> <adam.wulkiewicz_at_[hidden] <mailto:adam.wulkiewicz_at_[hidden]>> wrote:
>
>
<snip>
>
> I'm using a 2 arms, one 6 degree of freedom and the other 7 dof with 
> revolute (rotating) joints. The position of the arm can be accurately 
> represented as an n-d point representing the angle of each joint, with 
> no need for orientation/pose on the surface of a torus, with 1 
> dimension for each joint.
>
> Here is a simple image explaining the 2d case:
> http://robotics.stanford.edu/~latombe/cs326/2009/class3/class3.htm 
> <http://robotics.stanford.edu/%7Elatombe/cs326/2009/class3/class3.htm>
>
> Therefore I don't need to worry about orientation but probably just 
> the envelope/box which I believe is used for the data structure and 
> the points themselves.
>
>     Those 2d coordinates could be phi (toroidal angle) and theta
>     (poloidal angle)
>     (http://fusionwiki.ciemat.es/wiki/Toroidal_coordinates).
>
>
> As detailed above, this means in my current use case I can skip theta.
>
>     They could be stored in the rtree as 2d cartesian points but you'd
>     still have to implement your own distance function (see below). On
>     a torus those angles would probably both be in range [-pi, pi] and
>     both would have periodical nature (like longitude in spherical and
>     geographical coordinate systems).
>
>     Now it depends, how precise must the distance be. You could
>     calculate a simple cartesian distance of those coordinates
>     (treating them as cartesian) but taking the periodicity into
>     account. This however wouldn't be the "true"/shortest distance
>     between points or between a point and a box on the surface of a
>     torus, defined by geodesics. For this you'd probably have to
>     approximate it somehow through integral expression, newton's
>     method, taylor series, etc.
>
>
> I may be mistaken, but I believe the distance can be calculated by 
> taking the angular distance separately in each dimension as linked 
> previously, and listing them as a single tor_dist_vec, then take the 
> sqrt(element_sum(tor_dist_vec)). Since all the distances will always 
> be between [0,pi] I believe this will work correctly.

Ok, if I understand you correctly you would like to represent a 
configuration of an arm (N angles) as a point (N coordinates), store 
those points in the rtree and then use it to search for the closest 
known configuration.

If representing the configuration in 6d cartesian space is enough for 
you (which AFAIU is the case) then you just may store tham in the rtree 
as they are.

Now the question is, can the joints rotate infinitely or not (because of 
some mechanical limitations)? I.e. can a joint go from angle 0 to 2pi 
and then further so again jump to 0 and go to 2pi, etc.?

If the answer is no and the joint must go back then cartesian 
representation is all you need.

But if the answer is yes then the cartesan representation is not really 
correct, since the angles may wrap around the edges 0 <-> 2pi or -pi <-> 
pi and you should be able to find the closest distance if it was on the 
other side of the range. So indeed we have a torus, for 2 joints -> 2DoF 
-> 3D torus, so I'm guessing that 6 joints -> 6Dof -> 7D torus. In this 
case simple cartesian distance won't be sufficient because in any of 
those dimensions the coordinates can wrap around the edge, so you must 
take it into account. Which means that for each coordinate that can wrap 
you need to calculate a normalized signed difference in a range [-pi, 
pi] and calculate a distance using this value. For non-wrapping 
coordinate you just need a regular, not-normalized difference. 
Furthermore this can be a cartesian comparable_distance, so there is no 
need to calcualte the sqrt(). A sum of elements probably won't do the 
trick, you need a dot product so a sum of squares. This'd look like this 
in pseudo-code:

dot = 0;
foreach ( D )// you need to use a recursive templatefor this
{
     diff = get<D>(p2) - get<D>(p1);
     if ( is_wrapping )
         normalize(diff);// to range [-pi, pi]
dot += diff * diff;
}
return dot; // comparable_distance

Now, the above is for point-point distance. Note that this may not be 
the "correct" distance on a torus' surfce just like a cartesian distance 
of angular coordinates is not the "correct" distance on a sphere. I put 
the word "correct" in the quotes because what's correct depends on the 
case I guess.

But let's back to the subject. You also need similar implementation for 
point-box, so for each dimension first check if a point is closer to 
min, max or between them and then use the diff (min<D> - p<D>), (p<D> - 
max<D>), or 0 respectively. Again normalizing if the coordinates can be 
wrapped. In the case of cartesian points stored in the rtree, nodes' 
bounding boxes will also be cartesian so exactly in the same range [0, 
2pi] with min <= max. So there shouldn't be any additional problems with 
wrapping (this is true only for points stored in the rtree!). But if 
needed, normalize the box and point before checking if the point is 
between min and max, they must be represented somehow uniformly.

If some joints could be rotated e.g. only N times I'm guessing you could 
represent them by having the coordinates in the range [0, N*2pi] without 
wrapping.

Putting all of this together:

// define a separate point for this specific arm in order to calculate 
the distance specifically for it.
struct arm_6dof_pos { /*...*/ };
// register the above as 6D cartesian point

namespace boost { namespace geometry{

double comparable_distance(arm_6dof_pos const& p1, arm_6dof_pos const& p2)
{
     // calculate the point/point distance(above)
}

template <typename Box>
double comparable_distance(arm_6dof_pos const& p, Box const& b)
{
     // calculate the point/box distance
}

}} // namespace

/*...*/

rtree<arm_6dof_pos, ...>tree;
/*fill the rtree*/

arm_6dof_pos query_pos(/*...*/);
tree.query(nearest(query_pos, 1), out);

I hope that I understand your problem correctly and that the above is 
clear enough.
Please let me know about the outcome, it's very interesting!

>
>     The results of a quick search for the definition of geodesics on
>     torus:
>     http://www.rdrop.com/~half/math/torus/torus.geodesics.pdf
>     <http://www.rdrop.com/%7Ehalf/math/torus/torus.geodesics.pdf>
>     http://www34.homepage.villanova.edu/robert.jantzen/notes/torus/torusgeos.pdf
>>
> Thanks, these look like good links. Right now I only plan to use the 
> coordinates to find the k nearest neighbors in cartesian space, but 
> the information is useful to have.
>
>>
>>     Or you could consider contributing the support for another
>>     coordinate system.
>
> I might be interested in doing this eventually. Are the instructions 
> you have in that github repository under your account sufficiently up 
> to date for me to follow in getting set up? 
> https://github.com/awulkiew/temp-contributing

Yes, those are the instructions about the worflow we use at 
Boost.Geometry, how to work with modularized Boost, GitHub, etc.
As states on this page, the more recent version can be found in the 
Boost.Geometry wiki:
https://github.com/boostorg/geometry/wiki/Contribution-Tutorial

Regards,
Adam