(1) Add short-circuit to assemble() and track() methods of Assembler so that no optimization is performed if the state is already feasible and either has no goal or the goal is within tolerance of zero already.
(2) On return from the optimizer in assemble(), make sure that the solution wasn't made any worse if it started out feasible; IpOpt has been observed to do that. This is to avoid embarrassment! I did not make the same change to track() since there is a performance cost.

Fixes issue #167.
Merge pull request #168 from simbody/dontLetAssemblerMakeSolutionWorse-issue167

Don't let Assembler make the initial solution worse