STOMP alters initial and final states #14
Description
STOMP yields a trajectory whose initial and final states are different from the specified ones, sometimes by a large margin.
Stomp::updateParameters in stomp.cpp updates the parameters_optimized_ matrix with the calculated parameters_updates_. Since parameters_updates is fully populated, this effectively modifies the entire trajectory including initial and final states.