Once the CFS equation has been determined, the problem is almost solved. The action trajectory was determined from the given state trajectory and the backward P. Hall coordinate trajectory is determined by (15.143). The only remaining problem is that the action variables from to are fictitious because their associated vector fields are not part of the system. They were instead obtained from Lie bracket operations. When these are applied, they interfere with each other because many of them may try to use the same variables from the original system at the same time.

The CBHD formula is used to determine the solution in terms of the system action variables , , . The differential equation now becomes

which is initialized with and uses the original system instead of the extended system.

When applying vector fields over time, the CBHD formula becomes

If the system is nilpotent, then this series is truncated, and the exact effect of sequentially combining constant motion primitives can be determined. This leads to a procedure for determining a finite sequence of constant motion primitives that generate a motion in the same direction as prescribed by the extended system and the action trajectory .

Steven M LaValle 2012-04-20