Handle compound state space for AORRTC#1345
Conversation
|
Hi Jaeyoung, The AOX distance function should be using the same spatial distance function as euclidean, with the added cost distance. Please update the aoxDistanceFunction so that spaceDiff is calculated using your changed euclideanDistanceFunction (as opposed to si_->distance(...)) |
| if (stateSpace->isCompound()) | ||
| { | ||
| auto *compoundSpace = stateSpace->as<base::CompoundStateSpace>(); | ||
| // Get the first subspace which should be the position component (RealVectorStateSpace) |
There was a problem hiding this comment.
This is not guaranteed. Rephrase as:
| // Get the first subspace which should be the position component (RealVectorStateSpace) | |
| // Get the first subspace which should be the position component (RealVectorStateSpace) for state spaces like SE(2) and SE(3). |
| auto *subspace = compoundSpace->getSubspace(0).get(); | ||
| if (subspace->getType() == base::STATE_SPACE_REAL_VECTOR) | ||
| { | ||
| auto *rvSpace = subspace->as<base::RealVectorStateSpace>(); |
There was a problem hiding this comment.
No need to reimplement RealVectorStateSpace::distance. I think you can just call rvspace->distance(aCompound->as<base::RealVectorStateSpace::StateType>(0), bCompound->as<base::RealVectorStateSpace::StateType>(0))
|
It seems like this change should probably also print out a warning (once) when AORRTC is used with compound state spaces, since it's unclear (to me) what kind of optimality is guaranteed. If orientation distance doesn't matter, what would happen if SE(3)/SE(2) is set up with a weight of 0 for the orientation component (so that the distance is Euclidean distance). |
Problem Description
When using compound statespace(e.g. Dubins) with AORRTC #1282 the
euclideanDistanceFunctionwas using the distance metric of the state space rather than the euclidean distance function. (Probably assuming all spaces are metric and euclidean?)This PR checks whether the statespace is a compound statespace, and tries to extract the realvector statespace to compute the euclideandistance.
FYI @twill777