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

Currently for most of the problems we look at, loops (an edge going into
the same state) are not useful. However, these loops appear in a lot of
state sequences considered by the algorithm and slow it down
significantly. Until we encounter a problem where loops are necessary to
accomplish the task, we should just ignore them.

fix submodule url

  - Use parent implementation for method solve,
  - reimplement tryToConnectInitAndGoals,
  - remove InStatePath class and call core::DiffusingPlanner instead.
  - optimize paths on sub-manifolds with random shortcut and insert path
    in roadmap.

  when calling a path planner in hpp-core.
  Throw core::path_planning_failed when interrupted.

Fix steeringMethod::CrossStateOptimization

  - do not copy constraints from graph solvers,
  - in solver construction, use target constraints of transitions instead of
    merging solvers of edge, graph and target state. In target constraints,
    constraints and complements are merged when necessary.

  Do not clear constraintsAndComplements_ after graph initialization,
  Add method clearConstraintsAndComplement(),
  call this method in ProblemSolver::initConstraintGraph().

  - add parameter "CrossStateOptimization/nRandomConfig" to specify the number
    of trials to generate each waypoint configuration.

  if starting configuration slightly violates the constraint.
  Return false instead.

This reverts commit 0d81bac3.

  - only before initialization.

  - remove duplicate loop.