Change State type to support user defined data.

[Levi-Armstrong]

This would allow for metadata to be stored about the sampled state which can be used during edge evaluation. For example, I have a series of cartesian poses that can be rotated about the z-axis 0 or 180deg, but only 0's can be connected to each other and 180's to each other. This can not easily be recomputed when the path is along a highly contoured surface. @marip8 what do you think?

template <typename FloatType>
struct State
{
Eigen::Matrix<FloatType, Eigen::Dynamic, 1> state;
std::shared_ptr<void> user_data;
}

[marip8]

I'm ambivalent about adding a hook for custom user data in the state representation right now. It doesn't seem like a big addition, but it seems like we could solve the issue from your use-case and most others using the framework we already have in place without needing extra custom information. I want to avoid bloating the interfaces with extra optional information if it's not strictly necessary

Here are some alternative strategies for tackling your specific use-case:

1. Create a custom edge-evaluator that performs forward kinematics and compares orientation between the two waypoints to determine if a transition is possible
2. Solve two different planning problems, one for the 0 degree samples and one for the 180 degree samples.
   • In my opinion, the primary value of descartes is the search capability through a densely connected graph of state samples. If you're not going to allow dense connections, you may as well just run two separate planning problems in parallel and choose the best plan. This starts getting into the realm of meta-planning which would be a different discussion
3. Introduce a new graph type or graph builder interface that can consider special rules when making connections between nodes.
   • Currently we assume that state samples in adjacent rungs should be are densely connected, but we could allow for customization in the creation of edges. This might be difficult to do in practice, but it could be a viable option if we have enough use-cases where dense connection doesn't make sense

[Levi-Armstrong]

The current process of Descartes generating nodes more or less follows this sequence of steps

• Start with a cartesian Poses or Joint Poses
• If cartesian pose, we sample in cartesian space then solve IK and return joint solutions but the information about the sampled cartesian poses is lost.
• If joint pose we sample if allowed in joint space and return joint solutions

Now when you get to edges validation the information about the cartesian pose samples is lost because we only return the joint solution. I think it could be valuable for certain applications if you could do edge evaluation base on either the sampled cartesian pose, joint solution, or both. I think for this to be possible it requires changing Descartes interface to better support providing this information and thought a temporary solution would be to allow user-defined data to be stored with the state.

Create a custom edge-evaluator that performs forward kinematics and compares orientation between the two waypoints to determine if a transition is possible

This is originally what I tried but it fails to work because when you solve forward kinematics you still do not know the relationship of this cartesian pose to the nominal cartesian pose that the sample was created from. The example case is where you have a nominal pose and you sample at 10 degrees clockwise around z-axis to 360 degrees for every nominal pose. Now when it comes to determining if an edge should be connected I would like to do this base on the cartesian information. Where the joint solutions for the first sampled cartesian pose can only be connected to the first three sampled cartesian pose solutions. Basically creating a window relative to the previous cartesian pose.

Also, recalculating information when evaluating edges is not ideal because you end up having to calculate the same information for every edge compared and in large problems, this is a significant amount of time wasted. Where if you could store this information at the state this is prevented. I see other use case likes storing reachability, manipulability and other information as user data which could be used for edge evaluation.

Solve two different planning problems, one for the 0 degree samples and one for the 180 degree samples.
In my opinion, the primary value of Descartes is the search capability through a densely connected graph of state samples. If > you're not going to allow dense connections, you may as well just run two separate planning problems in parallel and choose > > the best plan. This starts getting into the realm of meta-planning which would be a different discussion

This is true for binary cases like 0 degrees or 180 degrees but does not apply if you have a window or range that is to be considered based on the connecting edge.

Looking at OMPL they have a base class for the State and then allow user implementation to store user data, then in the edge evaluator, they cast the State to their derived type to get access to the data they need for evaluation. Maybe this is a better approach? There is not a difference in functionality between the two, but more based on style preference.

[Levi-Armstrong]

https://github.com/ompl/ompl/blob/main/src/ompl/base/State.h

[marip8]

That's a fair point about storage of additional information and computation time. I would be interested to see a benchmark for the forward kinematics approach I proposed above; theoretically it would take more time to compute FK for each edge, but I wonder practically how much time is really added and if it makes a significant difference.

I would be open to making the State class a type-eraser. I think I like that approach better than adding a void pointer member to contain information. It would essentially function the same way, but I think the type-eraser approach seems cleaner. It should be easy to implement this with boost type erasure module

[Levi-Armstrong]

I also like the type-eraser approach.