for more information, see https://pre-commit.ci

The default timeout for innerPlanner is a positive value. However, if
the user specifically sets it to be 0, the timeout will not be set and
hence timeout will not be used to stop innerPlanner (default timeout in
PathPlanner is infinity).

buildOptProblem should allow the case when there is no waypoint to be
computed (only 1 loop transition from init config to the goal).

PathPlanner::tryConnectInitAndGoals() should be called when the goal
configuration is given.

The configSolved() method is redundant. Checks are added for the case
where the final configuration computed has been reached but the goal is
deemed not reached by the path planner.

Some other small redundancies are also removed.

Note that we need to set it **after** the problem's path projector has
been initialized.

Declaring a vector without explicitly initializing the values to 0 seems
to (sometimes) have strange behavior. This has been fixed.

Some comments are added to explain buildOptimizationProblem.

Previously, when the inStatePlanner fails to compute path between two
configurations on the same leaf, the roadmap generated is not added to
the main roadmap of StatesPathFinder. Now it is added so that the
roadmap gets expanded whenever inStatePlanner is used. The good side
effect is fairer comparison between different algorithms when comparing by the number
of nodes in the roadmap.

Clearer logic flows for SolveOptimizationProblem. Also, save the longest
sequence when we need to reset solution due to exceeding the maximum
fails.

Previously, we handled this case when goal is defined as a goal
configuration. However, this case may still occur when goal is defined
as a set of constraints. Moreover, the previous method does not store
the loop transition to the list of solutions, making it difficult to
re-try later on.

The graph has loop edge for every state (node), which can be used to
connect init and goal config directly if they are both in the same
state.

Information about all the nodes in the constraint graph would be useful
for debugging, eg when checking if the constraint graph is properly
constructed.

Previously, there are multiple versions of the same function, to cater
for two cases: when goal is defined as a configuration, and when when
goal is defined as a set of constraints. This duplication is not
desirable, as we need to make changes in multiple places if we want to
make a small change in a function. This commit removes the duplication
by merging the functions together and adding checks within the new
functions to cater for the two cases.

Some modifications are needed for analyseOptimizationProblem when goal
is given as a configuration. So far we have made a few, but there may be
more.

BUG: if a certain transition list succeeds to give a configuration list for
the first time, and fails for the second time, a new transition list
will be considered, but skipColAnalysis is NOT reset. This is now fixed
by using the direct comparison with nTryConfigList_ instead of using an
additional data member.

Even though bad solve statuses are usual because sometimes it is hard to
solve for the waypoint, there could be cases where it is impossible to
solve for a waypoint for a certain transition list (although we try to
ensure that the solver should have a positive probability of solving
it, by analysing the optimization problem in an earlier step). Adding a
threshold to it would ensure that the solving step does not take an
exponential time to decide that a transition list is not suitable.

Previously, `solveOptimizationProblem()` sometimes returns configuration
list that has collision in at least one configuration. This was possibly
because the collision validation is not working properly. Fixed by using
the new collision validation provided by hpp::core.

Some refactoring is also done to make the return of the function a bit
clearer (instead of simple int, use enum).

Make graph search data part of the path planner object, so that we don't
have to search the graph from scratch everytime we want to compute a new
list of configurations.

Remove nested if and add more comments to explain GraphSearchData

From the list of goal constraints, we can find a list of states that
contain the maximum number of goal constraints. We choose to compute
only state sequences that end in a state that can potentially be a goal
state. However, future work needs to assess/prove that this heuristic
covers all the possible goal states.

Previously the code has segmentation fault during one of the solutions
(not every one). This was because when an item is inserted into the
deque, iterators are invalidated. Incrementing the iterator causes
undefined behavior, which somehow worked until that point in the program
but not afterwards. We have changed to use index instead of iterator on
the deque.

When goal is defined as set of constraints, every state sequence is a
possible solution. The current implementation grows the list of possible
state sequences faster than they are attempted: for every time we add all
neighbors of the front node to queue, we only try out one sequences.
This could be too space consuming if the graph has large branching factor.

Non-static data members should be initialized in the order they were declared in
the class definition. For good practice, we should keep all the data
members in good order.

Before adding another constraint to a solver, check if it already has
the stricter version of the constraint. `constraint/hold` is stricter
than `constraint` and stricter than `constraint/complement`.

The expected behavior of solveOptimizationProblem () function is that,
if it manages to compute configurations for all waypoints, it will
return that sequence of configurations. In case when it fails, it
should return the longest sequence of valid configurations.

The previous implementation saves each configuration computed, even if
it is invalid. Also, when backtracking to a previous waypoint, the new
points computed will overwrite some of the waypoint in the longest
sequence. So indeed, the final sequence kept is not valid, and not
consistent (not generated in successful consecutive solves).

This has now been fixed by saving the longest sequence of valid
configurations in a separate variable, and only update it when a longer
sequence is found.

For the parametrizable constraints, we should initialize them before using
the solver to check if the initial configuration satisfies all the
propagated constraints.

Propagate the constraints from intermediate nodes as well, so that we
have a higher chance of filtering out invalid state sequences.
Direction of propagation should be from the final node to the first
node, so that we don't miss out the constraints that don't reach the
final node but are still relevant for the first portion of the
transitions.

This is currently done by propagating the constraints from the last
state to the initial state if the joints involved in each constraint are
constrained during all transitions between the initial state and the
last state. Checking the initial configuration to see if it
satisfies these constraints will help to eliminate some state sequences.

Able to build now, but code is probably useless