Andrew Hundt wrote:
On Thu, Apr 2, 2015 at 9:43 AM, Adam Wulkiewicz <adam.wulkiewicz@gmail.com> 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

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 template for 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!

 
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