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