Implicit metric in the tangent space about aikido

[aaronjoh]

Update: We are definitely assuming a euclidean metric induced from our parameterization of the tangent space. However, the implications are not too bad because we are picking the "right" parameterization for things like SO(3). The resolution of this issue requires:

• Document the tangent space parameterization used. The default should be nothing surprising:
  • Cartesian for R^n, with identical scale and alignment to the base
  • Angle in radians for SO(2)
  • Spacial twist for SE(3)
  • etc
• If you want a different parameterization (e.g. polar coordinates, Euler angles) then you need to implement a new statespace class.
• In distance metrics, have the default return a metric that is consistent with this assumption (the geodesic distance, induced by the parameterization of the tangent space mapped to a local region around a point, and then Euclidean distance on the tangent space parameterization). This will have a weighting of 1 between translation (in meters) and rotation (in radians) for SE(n).
• The default interpolator will use this parameterization. For example interpolating from (1,0) to (0,1) in cartesean coordinates on R2 at alpha=1/2 will get (1/2,1/2). If we had parameterized R2 in polar coordinates (\rho,\theta), this would be an interpolation from (1,0) to (1,\pi/2), and so the half way point would be (1,\pi/4) or (.707,.707) in Cartesian coordinates.
• Add a test to see if (for default returned metric d, state space interpolator i, initial points p1 and p2, and 0<a<1):

d(p1, i(p1,p2,a) ) = a * d(p1,p2)
d(i(p1,p2,a),p2) ) = (1-a) * d(p1,p2)

(There are other distance metrics where this wont be equality but rather triangle inequality, however I think our defaults should always satisfy this. The geodesic interpolator should follow a "straight line", so we can map that to R1, while the default distance metric is the geodesic distance along this line, and on R1 the Euclidean distance is additive).

• The distance metric for Rn will be Euclidean (L2 norm). If you want an L1 norm you should construct your space as a Cartesian product of R^1.
• We should also add some additional metrics that are ready to be used, like a weighted SE(3), L1 (or really L-p for some p) on Rn, etc

Sound good?