Bug with output vector disagreements

[ebianchi]

The C3 optimization solves for a vector stored as the internal class variable z_sol_, which contains the stacked vectors corresponding to x, u, lambda, (and for C3+ eta). While z_sol_ contains the most up-to-date values, the other class variables x_sol_, u_sol_, and lambda_sol_ do not always reflect these same values -- they are the same at the end of a call to C3::Solve(...) (as ensured by this call) only if the option end_on_qp_step is true (which otherwise updates the z_sol_ vector here without also updating the x_sol_, etc. vectors).

Given the intuitive outputs of C3 are the state and input (and possibly contact force) vectors accessible via the helpers GetStateSolution and GetInputSolution (and GetForceSolution), respectively, these x_sol_, u_sol_, and lambda_sol_ vectors should reflect the most updated values in z_sol_.

Proposed solution:

• Remove the class variables x_sol_, u_sol_, and lambda_sol_.
• Edit the helper functions GetStateSolution, GetInputSolution, and GetForceSolution to grab and return the relevant portions of the z_sol_ vector when called.

[ebianchi]

Upon further reflection, it may be desirable for x_sol_, u_sol_, lambda_sol_ to exist in addition to z_sol_, since the latter is updated throughout the ADMM iterations, and the former can be set at the end of each C3 solve, reflecting the final values of z_sol_.

Updated proposed solution:

• Keep the class variables x_sol_, u_sol_, and lambda_sol_ and their corresponding helper function definitions.
• Make the last step of C3::Solve(...) set these class variables according to the final z_sol_ values.
• Remove the is_final_solve input argument to C3::StoreQPResults(...), since it is now the job of C3::Solve(...) to set x_sol_, u_sol_, and lambda_sol_.