Distance between angles as points
This code is very hard to read. These are very simple geometric operations that you are programming (just things like subtracting vectors, or finding a vector's squared length) but their meaning is buried under a mountain of boilerplate.

Where you have:

boost::transform(p1,p2,diff.begin(),std::minus&lt;ArmPos::value_type&gt;());


could you find some way to write:

diff = p1 - p2;


instead? Similarly, where you have:

return boost::accumulate(diff,0,square&lt;ArmPos::value_type&gt;());


could you find some way to write:

return diff.lengthSquared();


instead? C++ supports operator overloading and virtual functions, so surely something along these lines would be possible.