From 8fb545253d23130e18211bdcc9c288521441b996 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 11 Jul 2019 16:12:14 -0700 Subject: [PATCH 1/5] Fooling around with scaling in tropter. --- .../tropter/optimization/AbstractProblem.h | 109 ++++++++++++------ tropter/tropter/optimization/IPOPTSolver.cpp | 6 +- tropter/tropter/optimization/IPOPTSolver.h | 2 +- .../tropter/optimization/ProblemDecorator.h | 54 ++++++++- .../optimization/ProblemDecorator_double.cpp | 29 +++-- tropter/tropter/optimization/Solver.cpp | 7 +- tropter/tropter/optimization/Solver.h | 2 +- 7 files changed, 156 insertions(+), 53 deletions(-) diff --git a/tropter/tropter/optimization/AbstractProblem.h b/tropter/tropter/optimization/AbstractProblem.h index d79413b4c..0197b4965 100644 --- a/tropter/tropter/optimization/AbstractProblem.h +++ b/tropter/tropter/optimization/AbstractProblem.h @@ -16,10 +16,11 @@ // limitations under the License. // ---------------------------------------------------------------------------- -#include -#include #include +#include +#include + namespace tropter { class SymmetricSparsityPattern; @@ -31,22 +32,37 @@ class ProblemDecorator; /// @ingroup optimization class AbstractProblem { public: - AbstractProblem() = default; AbstractProblem(unsigned num_variables, unsigned num_constraints) - :m_num_variables(num_variables), - m_num_constraints(num_constraints) { } + : m_num_variables(num_variables), + m_num_constraints(num_constraints) {} virtual ~AbstractProblem() = default; unsigned get_num_variables() const { return m_num_variables; } unsigned get_num_constraints() const { return m_num_constraints; } - const Eigen::VectorXd& - get_variable_lower_bounds() const { return m_variable_lower_bounds; } - const Eigen::VectorXd& - get_variable_upper_bounds() const { return m_variable_upper_bounds; } - const Eigen::VectorXd& - get_constraint_lower_bounds() const { return m_constraint_lower_bounds; } - const Eigen::VectorXd& - get_constraint_upper_bounds() const { return m_constraint_upper_bounds; } + const Eigen::VectorXd& get_variable_lower_bounds() const { + return m_variable_lower_bounds; + } + const Eigen::VectorXd& get_variable_upper_bounds() const { + return m_variable_upper_bounds; + } + + bool has_variable_scaling() const { + return m_variable_scaling_shift.size() && + m_variable_scaling_scale.size(); + } + const Eigen::VectorXd& get_variable_scaling_shift() const { + return m_variable_scaling_shift; + } + const Eigen::VectorXd& get_variable_scaling_scale() const { + return m_variable_scaling_scale; + } + + const Eigen::VectorXd& get_constraint_lower_bounds() const { + return m_constraint_lower_bounds; + } + const Eigen::VectorXd& get_constraint_upper_bounds() const { + return m_constraint_upper_bounds; + } /// Get a vector of names of all variables in the optimization problem, /// in the correct order. @@ -80,14 +96,16 @@ class AbstractProblem { /// implementing calc_sparsity_hessian_lagrangian())? If false, then we /// assume the Hessian is dense, which will have a very negative impact /// on performance. - bool get_use_supplied_sparsity_hessian_lagrangian() const - { return m_use_supplied_sparsity_hessian_lagrangian; } + bool get_use_supplied_sparsity_hessian_lagrangian() const { + return m_use_supplied_sparsity_hessian_lagrangian; + } /// @copydoc get_use_supplied_sparsity_hessian_lagrangian() /// If this is true and calc_sparsity_hessian_lagrangian() is not /// implemented, an exception is thrown. /// This must be false if using automatic differentiation. - void set_use_supplied_sparsity_hessian_lagrangian(bool value) - { m_use_supplied_sparsity_hessian_lagrangian = value; } + void set_use_supplied_sparsity_hessian_lagrangian(bool value) { + m_use_supplied_sparsity_hessian_lagrangian = value; + } /// If using finite differences (double) with a Newton method (exact /// Hessian in IPOPT), then we require the sparsity pattern of the /// Hessian of the Lagrangian. By default, we estimate the Hessian's @@ -111,11 +129,9 @@ class AbstractProblem { class CalcSparsityHessianLagrangianNotImplemented : public Exception {}; - virtual std::unique_ptr - make_decorator() const = 0; + virtual std::unique_ptr make_decorator() const = 0; protected: - void set_num_variables(unsigned num_variables) { // TODO if set, invalidate variable bounds. m_num_variables = num_variables; @@ -123,8 +139,8 @@ class AbstractProblem { void set_num_constraints(unsigned num_constraints) { m_num_constraints = num_constraints; } - void set_variable_bounds(const Eigen::VectorXd& lower, - const Eigen::VectorXd& upper) { + void set_variable_bounds( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper) { // TODO make sure num_variables has been set. // TODO can only call this if m_num_variables etc are already set. assert(lower.size() == m_num_variables); @@ -135,8 +151,33 @@ class AbstractProblem { m_variable_lower_bounds = lower; m_variable_upper_bounds = upper; } - void set_constraint_bounds(const Eigen::VectorXd& lower, - const Eigen::VectorXd& upper) { + + void set_variable_scaling_from_range( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper) { + // Betts 2010 page 167. + set_variable_scaling(1.0 / (upper.array() - lower.array()), + 0.5 - upper.array() / (upper.array() - lower.array())); + } + // TODO must have already called set_variable_bounds(). + void set_variable_scaling_from_bounds() { + TROPTER_THROW_IF((m_variable_lower_bounds.size() == 0 || + m_variable_upper_bounds.size() == 0) && + m_num_variables, + "To set variable scaling from bounds, bounds must be set."); + set_variable_scaling_from_range( + m_variable_lower_bounds, m_variable_upper_bounds); + } + // TODO + void set_variable_scaling( + const Eigen::VectorXd& shift, const Eigen::VectorXd& scale) { + assert(shift.size() == m_num_variables); + assert(scale.size() == m_num_variables); + assert((scale.array() > 0).all()); + m_variable_scaling_shift = shift; + m_variable_scaling_scale = scale; + } + void set_constraint_bounds( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper) { assert(lower.size() == m_num_constraints); assert(upper.size() == m_num_constraints); // TODO assert(lower <= upper); @@ -153,17 +194,17 @@ class AbstractProblem { Eigen::VectorXd m_variable_upper_bounds; Eigen::VectorXd m_constraint_lower_bounds; Eigen::VectorXd m_constraint_upper_bounds; + + Eigen::VectorXd m_variable_scaling_shift; + Eigen::VectorXd m_variable_scaling_scale; }; inline void AbstractProblem::calc_sparsity_hessian_lagrangian( - const Eigen::VectorXd&, - SymmetricSparsityPattern&, + const Eigen::VectorXd&, SymmetricSparsityPattern&, SymmetricSparsityPattern&) const { throw CalcSparsityHessianLagrangianNotImplemented(); } -inline Eigen::VectorXd -AbstractProblem::make_initial_guess_from_bounds() const -{ +inline Eigen::VectorXd AbstractProblem::make_initial_guess_from_bounds() const { const auto& lower = get_variable_lower_bounds(); const auto& upper = get_variable_upper_bounds(); assert(lower.size() == upper.size()); @@ -172,10 +213,12 @@ AbstractProblem::make_initial_guess_from_bounds() const for (Eigen::Index i = 0; i < lower.size(); ++i) { if (lower[i] != -inf && upper[i] != inf) { guess[i] = 0.5 * (upper[i] + lower[i]); - } - else if (lower[i] != -inf) guess[i] = lower[i]; - else if (upper[i] != inf) guess[i] = upper[i]; - else guess[i] = 0; + } else if (lower[i] != -inf) + guess[i] = lower[i]; + else if (upper[i] != inf) + guess[i] = upper[i]; + else + guess[i] = 0; } return guess; } diff --git a/tropter/tropter/optimization/IPOPTSolver.cpp b/tropter/tropter/optimization/IPOPTSolver.cpp index df5d6692b..cde15398c 100644 --- a/tropter/tropter/optimization/IPOPTSolver.cpp +++ b/tropter/tropter/optimization/IPOPTSolver.cpp @@ -164,7 +164,7 @@ std::string convert_IPOPT_ApplicationReturnStatus_to_string( } } -Solution IPOPTSolver::optimize_impl(const VectorXd& guess) const { +Solution IPOPTSolver::optimize_impl(const VectorXd& scaled_guess) const { Ipopt::SmartPtr app = IpoptApplicationFactory(); // Set options. @@ -266,9 +266,9 @@ Solution IPOPTSolver::optimize_impl(const VectorXd& guess) const { // Determine sparsity pattern of Jacobian, Hessian, etc. SparsityCoordinates jacobian_sparsity; SparsityCoordinates hessian_sparsity; - calc_sparsity(guess, jacobian_sparsity, + calc_sparsity(scaled_guess, jacobian_sparsity, need_exact_hessian, hessian_sparsity); - nlp->initialize(guess, std::move(jacobian_sparsity), + nlp->initialize(scaled_guess, std::move(jacobian_sparsity), std::move(hessian_sparsity)); // Optimize!!! diff --git a/tropter/tropter/optimization/IPOPTSolver.h b/tropter/tropter/optimization/IPOPTSolver.h index 34a6a3204..977f63441 100644 --- a/tropter/tropter/optimization/IPOPTSolver.h +++ b/tropter/tropter/optimization/IPOPTSolver.h @@ -47,7 +47,7 @@ class IPOPTSolver : public Solver { // TODO cannot use temporary. static void print_available_options(); protected: - Solution optimize_impl(const Eigen::VectorXd& guess) const override; + Solution optimize_impl(const Eigen::VectorXd& scaled_guess) const override; void get_available_options( std::vector&, std::vector&, std::vector&) const override; diff --git a/tropter/tropter/optimization/ProblemDecorator.h b/tropter/tropter/optimization/ProblemDecorator.h index 59155036c..44f2412d5 100644 --- a/tropter/tropter/optimization/ProblemDecorator.h +++ b/tropter/tropter/optimization/ProblemDecorator.h @@ -44,6 +44,7 @@ class ProblemDecorator { { return m_problem.get_num_variables(); } unsigned get_num_constraints() const { return m_problem.get_num_constraints(); } + // TODO must scale these! const Eigen::VectorXd& get_variable_lower_bounds() const { return m_problem.get_variable_lower_bounds(); } const Eigen::VectorXd& get_variable_upper_bounds() const @@ -67,13 +68,20 @@ class ProblemDecorator { /// @see AbstractOptimizationProblem::make_random_iterate_within_bounds() Eigen::VectorXd make_random_iterate_within_bounds() const { return m_problem.make_random_iterate_within_bounds(); } + + void scale_variables(const double* unscaled_variables, + double* scaled_variables) const; + Eigen::VectorXd scale_variables(Eigen::Ref unscaled) const; + void unscale_variables(const double* scaled_variables, + double* unscaled_variables) const; + Eigen::VectorXd unscale_variables(Eigen::Ref scaled) const; /// This function determines the sparsity pattern of the Jacobian and /// Hessian, using the provided variables. /// You must call this function first before calling calc_objective(), /// calc_constraints(), etc. // TODO create a struct to hold row and col indices. // TODO b/c of SNOPT, want to be able to ask for sparsity separately. - virtual void calc_sparsity(const Eigen::VectorXd& variables, + virtual void calc_sparsity(const Eigen::VectorXd& scaled_variables, SparsityCoordinates& jacobian_sparsity, bool provide_hessian_sparsity, SparsityCoordinates& hessian_sparsity) const = 0; @@ -130,6 +138,50 @@ class ProblemDecorator { std::string m_findiff_hessian_mode = "fast"; }; +inline void ProblemDecorator::scale_variables(const double* unscaled_variables, + double* scaled_variables) const { + int N = m_problem.get_num_variables(); + Eigen::Map unscaled(unscaled_variables, N); + Eigen::Map scaled(scaled_variables, N); + + if (m_problem.has_variable_scaling()) { + const auto& shift = m_problem.get_variable_scaling_shift(); + const auto& scale = m_problem.get_variable_scaling_scale(); + scaled = unscaled.array() * scale.array() + shift.array(); + } else if (scaled_variables != unscaled_variables) { + scaled = unscaled; + } +} + +inline Eigen::VectorXd ProblemDecorator::scale_variables( + Eigen::Ref unscaled) const { + Eigen::VectorXd out(m_problem.get_num_variables()); + scale_variables(unscaled.data(), out.data()); + return out; +} + +inline void ProblemDecorator::unscale_variables(const double* scaled_variables, + double* unscaled_variables) const { + int N = m_problem.get_num_variables(); + Eigen::Map scaled(scaled_variables, N); + Eigen::Map unscaled(unscaled_variables, N); + + if (m_problem.has_variable_scaling()) { + const auto& shift = m_problem.get_variable_scaling_shift(); + const auto& scale = m_problem.get_variable_scaling_scale(); + unscaled = (scaled.array() - shift.array()) / scale.array(); + } else if (scaled_variables != unscaled_variables) { + unscaled = scaled; + } +} + +inline Eigen::VectorXd ProblemDecorator::unscale_variables( + Eigen::Ref scaled) const { + Eigen::VectorXd out(m_problem.get_num_variables()); + unscale_variables(scaled.data(), out.data()); + return out; +} + inline int ProblemDecorator::get_verbosity() const { return m_verbosity; } inline double ProblemDecorator::get_findiff_hessian_step_size() const diff --git a/tropter/tropter/optimization/ProblemDecorator_double.cpp b/tropter/tropter/optimization/ProblemDecorator_double.cpp index 6a374d7ad..1b3506674 100644 --- a/tropter/tropter/optimization/ProblemDecorator_double.cpp +++ b/tropter/tropter/optimization/ProblemDecorator_double.cpp @@ -55,13 +55,14 @@ Problem::Decorator::Decorator( ProblemDecorator(problem), m_problem(problem) {} void Problem::Decorator:: -calc_sparsity(const Eigen::VectorXd& variables, +calc_sparsity(const Eigen::VectorXd& scaled_variables, SparsityCoordinates& jacobian_sparsity_coordinates, bool provide_hessian_sparsity, SparsityCoordinates& hessian_sparsity_coordinates) const { const auto num_vars = get_num_variables(); - m_x_working = VectorXd::Zero(num_vars); + m_x_working.resize(num_vars); + unscale_variables(scaled_variables.data(), m_x_working.data()); // Gradient. // ========= @@ -74,7 +75,7 @@ calc_sparsity(const Eigen::VectorXd& variables, return obj_value; }; SparsityPattern gradient_sparsity = - calc_gradient_sparsity_with_perturbation(variables, + calc_gradient_sparsity_with_perturbation(m_x_working, calc_objective); m_gradient_nonzero_indices = gradient_sparsity.convert_to_CompressedRowSparsity()[0]; @@ -95,7 +96,7 @@ calc_sparsity(const Eigen::VectorXd& variables, const auto var_names = m_problem.get_variable_names(); const auto constr_names = m_problem.get_constraint_names(); SparsityPattern jacobian_sparsity = - calc_jacobian_sparsity_with_perturbation(variables, + calc_jacobian_sparsity_with_perturbation(m_x_working, num_jac_rows, calc_constraints, constr_names, var_names); m_jacobian_coloring.reset(new JacobianColoring(jacobian_sparsity)); @@ -112,7 +113,7 @@ calc_sparsity(const Eigen::VectorXd& variables, // Hessian. // ======== if (provide_hessian_sparsity) { - calc_sparsity_hessian_lagrangian(variables, + calc_sparsity_hessian_lagrangian(m_x_working, hessian_sparsity_coordinates); // TODO produce a more informative number/description. @@ -132,6 +133,7 @@ calc_sparsity(const Eigen::VectorXd& variables, void Problem::Decorator:: calc_sparsity_hessian_lagrangian(const VectorXd& x, SparsityCoordinates& hessian_sparsity_coordinates) const { + // TODO scale? const int num_vars = (int)m_problem.get_num_variables(); SymmetricSparsityPattern hescon_sparsity(num_vars); @@ -199,9 +201,10 @@ calc_objective(unsigned num_variables, const double* variables, double& obj_value) const { obj_value = 0.0; - // TODO avoid copy. - const VectorXd xvec = Eigen::Map(variables, num_variables); - m_problem.calc_objective(xvec, obj_value); + // TODO: Use working memory. + VectorXd unscaled_variables(num_variables); + unscale_variables(variables, unscaled_variables.data()); + m_problem.calc_objective(unscaled_variables, obj_value); } void Problem::Decorator:: @@ -209,8 +212,8 @@ calc_constraints(unsigned num_variables, const double* variables, bool /*new_variables*/, unsigned num_constraints, double* constr) const { - // TODO avoid copy. - m_x_working = Eigen::Map(variables, num_variables); + m_x_working.resize(num_variables); + unscale_variables(variables, m_x_working.data()); VectorXd constrvec(num_constraints); // TODO avoid copy. // TODO at least keep constrvec as working memory. m_problem.calc_constraints(m_x_working, constrvec); @@ -279,9 +282,11 @@ calc_jacobian(unsigned num_variables, const double* variables, bool /*new_x*/, for (Eigen::Index iseed = 0; iseed < num_seeds; ++iseed) { const auto direction = seed.col(iseed); // Perturb x in the positive direction. - m_problem.calc_constraints(x0 + eps * direction, m_constr_pos); + m_problem.calc_constraints( + unscale_variables(x0 + eps * direction), m_constr_pos); // Perturb x in the negative direction. - m_problem.calc_constraints(x0 - eps * direction, m_constr_neg); + m_problem.calc_constraints( + unscale_variables(x0 - eps * direction), m_constr_neg); // Compute central difference. m_jacobian_compressed.col(iseed) = (m_constr_pos - m_constr_neg) / two_eps; diff --git a/tropter/tropter/optimization/Solver.cpp b/tropter/tropter/optimization/Solver.cpp index 5a06a3036..56cf4ef9c 100644 --- a/tropter/tropter/optimization/Solver.cpp +++ b/tropter/tropter/optimization/Solver.cpp @@ -177,6 +177,8 @@ Solver::optimize(const Eigen::VectorXd& variables) const TROPTER_THROW_IF(variables.size() != m_problem->get_num_variables(), "Expected guess to have %i elements, but it has %i elements.", m_problem->get_num_variables(), variables.size() ); + Eigen::VectorXd scaled_variables(variables.size()); + m_problem->scale_variables(variables.data(), scaled_variables.data()); return optimize_impl(variables); } @@ -186,7 +188,7 @@ Solver::optimize() const { return optimize_impl(m_problem->make_initial_guess_from_bounds()); } -void Solver::calc_sparsity(const Eigen::VectorXd guess, +void Solver::calc_sparsity(const Eigen::VectorXd scaled_guess, SparsityCoordinates& jacobian_sparsity, bool provide_hessian_sparsity, SparsityCoordinates& hessian_sparsity @@ -194,9 +196,10 @@ void Solver::calc_sparsity(const Eigen::VectorXd guess, VectorXd variables_random; const VectorXd* variables_for_sparsity = nullptr; if (get_sparsity_detection() == "initial-guess") { - variables_for_sparsity = &guess; + variables_for_sparsity = &scaled_guess; } else if (get_sparsity_detection() == "random") { variables_random = m_problem->make_random_iterate_within_bounds(); + m_problem->scale_variables(variables_random.data(), variables_random.data()); variables_for_sparsity = &variables_random; } assert(variables_for_sparsity); diff --git a/tropter/tropter/optimization/Solver.h b/tropter/tropter/optimization/Solver.h index 502726750..4ca7cc2c3 100644 --- a/tropter/tropter/optimization/Solver.h +++ b/tropter/tropter/optimization/Solver.h @@ -164,7 +164,7 @@ class Solver { /// be used. /// @throws if provide_hessian_indices is false but the decorator provides /// Hessian indices. - void calc_sparsity(const Eigen::VectorXd guess, + void calc_sparsity(const Eigen::VectorXd scaled_guess, SparsityCoordinates& jacobian_sparsity, bool provide_hessian_sparsity, SparsityCoordinates& hessian_sparsity) const; From bb64d707df78334b2a835f98f503afb8a6f338ff Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 11 Jul 2019 23:31:44 -0700 Subject: [PATCH 2/5] First pass at variable scaling for CasADi. Hinders performance. --- .../exampleSlidingMass/exampleSlidingMass.cpp | 4 +- Moco/Moco/MocoCasADiSolver/CasOCSolver.h | 9 ++ .../MocoCasADiSolver/CasOCTranscription.cpp | 126 ++++++++++++------ .../MocoCasADiSolver/CasOCTranscription.h | 50 +++++-- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 2 + Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.h | 2 + Moco/Moco/MocoProblem.cpp | 2 +- .../tropter/optimization/AbstractProblem.h | 2 + 8 files changed, 145 insertions(+), 52 deletions(-) diff --git a/Moco/Examples/C++/exampleSlidingMass/exampleSlidingMass.cpp b/Moco/Examples/C++/exampleSlidingMass/exampleSlidingMass.cpp index 5ba547002..37a0a2ccf 100644 --- a/Moco/Examples/C++/exampleSlidingMass/exampleSlidingMass.cpp +++ b/Moco/Examples/C++/exampleSlidingMass/exampleSlidingMass.cpp @@ -85,10 +85,10 @@ int main() { problem.setTimeBounds(MocoInitialBounds(0), MocoFinalBounds(0, 5)); // Initial position must be 0, final position must be 1. - problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + problem.setStateInfo("/slider/position/value", MocoBounds(0, 1), MocoInitialBounds(0), MocoFinalBounds(1)); // Initial and final speed must be 0. Use compact syntax. - problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); + problem.setStateInfo("/slider/position/speed", {-10, 10}, 0, 0); // Applied force must be between -50 and 50. problem.setControlInfo("/actuator", MocoBounds(-50, 50)); diff --git a/Moco/Moco/MocoCasADiSolver/CasOCSolver.h b/Moco/Moco/MocoCasADiSolver/CasOCSolver.h index 01cdc84e0..ca5f44442 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCSolver.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCSolver.h @@ -49,6 +49,14 @@ class Solver { const std::string& getTranscriptionScheme() const { return m_transcriptionScheme; } + /// Should variables be scaled by the variable bounds? + /// TODO explain more. + void setScaleVariablesUsingBounds(bool value) { + m_scaleVariablesUsingBounds = value; + } + bool getScaleVariablesUsingBounds() const { + return m_scaleVariablesUsingBounds; + } std::string getDynamicsMode() const { return m_problem.getDynamicsMode(); } bool isDynamicsModeImplicit() const { return m_problem.getDynamicsMode() == "implicit"; @@ -142,6 +150,7 @@ class Solver { const Problem& m_problem; std::vector m_mesh; std::string m_transcriptionScheme = "hermite-simpson"; + bool m_scaleVariablesUsingBounds = false; bool m_minimizeLagrangeMultipliers = false; double m_lagrangeMultiplierWeight = 1.0; bool m_interpolateControlMidpoints = true; diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 83132e62e..c5d81ccd3 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -81,7 +81,8 @@ class NlpsolCallback : public casadi::Callback { }; void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, - int numDefectsPerMeshInterval, const casadi::DM& pointsForInterpControls) { + int numDefectsPerMeshInterval, + const casadi::DM& pointsForInterpControls) { // Set the grid. // ------------- // The grid for a transcription scheme includes both mesh points (i.e. @@ -111,28 +112,22 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- - m_vars[initial_time] = MX::sym("initial_time"); - m_vars[final_time] = MX::sym("final_time"); - m_duration = m_vars[final_time] - m_vars[initial_time]; - m_times = createTimes(m_vars[initial_time], m_vars[final_time]); - m_vars[states] = + m_scaledVars[initial_time] = MX::sym("initial_time"); + m_scaledVars[final_time] = MX::sym("final_time"); + m_scaledVars[states] = MX::sym("states", m_problem.getNumStates(), m_numGridPoints); - m_vars[controls] = + m_scaledVars[controls] = MX::sym("controls", m_problem.getNumControls(), m_numGridPoints); - m_vars[multipliers] = MX::sym( + m_scaledVars[multipliers] = MX::sym( "multipliers", m_problem.getNumMultipliers(), m_numGridPoints); - m_vars[derivatives] = MX::sym( + m_scaledVars[derivatives] = MX::sym( "derivatives", m_problem.getNumDerivatives(), m_numGridPoints); // TODO: This assumes that slack variables are applied at all // collocation points on the mesh interval interior. - m_vars[slacks] = MX::sym( + m_scaledVars[slacks] = MX::sym( "slacks", m_problem.getNumSlacks(), m_numPointsIgnoringConstraints); - m_vars[parameters] = MX::sym("parameters", m_problem.getNumParameters(), 1); - - m_paramsTrajGrid = MX::repmat(m_vars[parameters], 1, m_numGridPoints); - m_paramsTraj = MX::repmat(m_vars[parameters], 1, m_numMeshPoints); - m_paramsTrajIgnoringConstraints = - MX::repmat(m_vars[parameters], 1, m_numPointsIgnoringConstraints); + m_scaledVars[parameters] = + MX::sym("parameters", m_problem.getNumParameters(), 1); m_kinematicConstraintIndices = createKinematicConstraintIndices(); std::vector daeIndicesVector; @@ -161,13 +156,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Set variable bounds. // -------------------- - auto initializeBounds = [&](VariablesDM& bounds) { - for (auto& kv : m_vars) { + auto initializeVariablesDM = [&](VariablesDM& bounds) { + for (auto& kv : m_scaledVars) { bounds[kv.first] = DM(kv.second.rows(), kv.second.columns()); } }; - initializeBounds(m_lowerBounds); - initializeBounds(m_upperBounds); + initializeVariablesDM(m_lowerBounds); + initializeVariablesDM(m_upperBounds); setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); @@ -231,6 +226,54 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ++ip; } } + + // Handle scaling of variables. + // ---------------------------- + initializeVariablesDM(m_shift); + initializeVariablesDM(m_scale); + + for (const auto& kv : m_scaledVars) { + const auto& key = kv.first; + // TODO: do NOT use initial/final bounds for scaling; always use phase + // bounds. + if (m_solver.getScaleVariablesUsingBounds()) { + const auto& L = m_lowerBounds.at(key); + const auto& U = m_upperBounds.at(key); + for (int irow = 0; irow < kv.second.rows(); ++irow) { + for (int icol = 0; icol < kv.second.columns(); ++icol) { + const double upper = U(irow, icol).scalar(); + const double lower = L(irow, icol).scalar(); + double range = upper - lower; + double midpoint; + if (std::isinf(range)) { + range = 1; + midpoint = 0; + } else if (range == 0) { + range = 1; + midpoint = upper; + } else { + midpoint = -0.5 * (upper + lower); + } + m_scale.at(key)(irow, icol) = range; + m_shift.at(key)(irow, icol) = midpoint; + } + } + } else { + m_scale.at(key) = 1; + m_shift.at(key) = 0; + } + } + + m_unscaledVars = unscaleVariables(m_scaledVars); + + m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; + m_times = createTimes( + m_unscaledVars[initial_time], m_unscaledVars[final_time]); + m_paramsTrajGrid = + MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); + m_paramsTraj = MX::repmat(m_unscaledVars[parameters], 1, m_numMeshPoints); + m_paramsTrajIgnoringConstraints = MX::repmat( + m_unscaledVars[parameters], 1, m_numPointsIgnoringConstraints); } void Transcription::transcribe() { @@ -285,7 +328,7 @@ void Transcription::transcribe() { // qdot // ---- - const MX u = m_vars[states](Slice(NQ, NQ + NU), Slice()); + const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); m_xdot(Slice(0, NQ), Slice()) = u; if (m_problem.getEnforceConstraintDerivatives() && @@ -311,7 +354,7 @@ void Transcription::transcribe() { if (m_solver.isDynamicsModeImplicit()) { // udot. - const MX w = m_vars[derivatives]; + const MX w = m_unscaledVars[derivatives]; m_xdot(Slice(NQ, NQ + NU), Slice()) = w; std::vector inputs{states, controls, multipliers, derivatives}; @@ -427,7 +470,7 @@ void Transcription::setObjective() { // Minimize Lagrange multipliers if specified by the solver. if (m_solver.getMinimizeLagrangeMultipliers() && m_problem.getNumMultipliers()) { - const auto mults = m_vars[multipliers]; + const auto mults = m_unscaledVars[multipliers]; const double multiplierWeight = m_solver.getLagrangeMultiplierWeight(); // Sum across constraints of each multiplier element squared. integrandTraj += multiplierWeight * MX::sum1(MX::sq(mults)); @@ -436,10 +479,11 @@ void Transcription::setObjective() { MXVector endpointCostOut; m_problem.getEndpointCost().call( - {m_vars[final_time], m_vars[states](Slice(), -1), - m_vars[controls](Slice(), -1), - m_vars[multipliers](Slice(), -1), - m_vars[derivatives](Slice(), -1), m_vars[parameters]}, + {m_unscaledVars[final_time], m_unscaledVars[states](Slice(), -1), + m_unscaledVars[controls](Slice(), -1), + m_unscaledVars[multipliers](Slice(), -1), + m_unscaledVars[derivatives](Slice(), -1), + m_unscaledVars[parameters]}, endpointCostOut); const auto endpointCost = endpointCostOut.at(0); @@ -447,6 +491,7 @@ void Transcription::setObjective() { } Solution Transcription::solve(const Iterate& guessOrig) { + // TODO scale guess! // Define the NLP. // --------------- @@ -497,7 +542,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { options[m_solver.getOptimSolver()] = m_solver.getSolverOptions(); } - auto x = flattenVariables(m_vars); + auto x = flattenVariables(m_scaledVars); casadi_int numVariables = x.numel(); // The m_constraints symbolic vector holds all of the expressions for @@ -539,18 +584,18 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Run the optimization (evaluate the CasADi NLP function). // -------------------------------------------------------- // The inputs and outputs of nlpFunc are numeric (casadi::DM). - const casadi::DMDict nlpResult = - nlpFunc(casadi::DMDict{{"x0", flattenVariables(guess.variables)}, - {"lbx", flattenVariables(m_lowerBounds)}, - {"ubx", flattenVariables(m_upperBounds)}, - {"lbg", flattenConstraints(m_constraintsLowerBounds)}, - {"ubg", flattenConstraints(m_constraintsUpperBounds)}}); + const casadi::DMDict nlpResult = nlpFunc(casadi::DMDict{ + {"x0", flattenVariables(scaleVariables(guess.variables))}, + {"lbx", flattenVariables(scaleVariables(m_lowerBounds))}, + {"ubx", flattenVariables(scaleVariables(m_upperBounds))}, + {"lbg", flattenConstraints(m_constraintsLowerBounds)}, + {"ubg", flattenConstraints(m_constraintsUpperBounds)}}); // Create a CasOC::Solution. // ------------------------- Solution solution = m_problem.createIterate(); const auto finalVariables = nlpResult.at("x"); - solution.variables = expandVariables(finalVariables); + solution.variables = unscaleVariables(expandVariables(finalVariables)); solution.objective = nlpResult.at("f").scalar(); solution.times = createTimes( solution.variables[initial_time], solution.variables[final_time]); @@ -566,8 +611,8 @@ Solution Transcription::solve(const Iterate& guessOrig) { return solution; } -void Transcription::printConstraintValues( - const Iterate& it, const Constraints& constraints, +void Transcription::printConstraintValues(const Iterate& it, + const Constraints& constraints, std::ostream& stream) const { // We want to be able to restore the stream's original formatting. @@ -1015,11 +1060,12 @@ casadi::MXVector Transcription::evalOnTrajectory( if (inputs[i] == multibody_states) { const auto NQ = m_problem.getNumCoordinates(); const auto NU = m_problem.getNumSpeeds(); - mxIn[i + 1] = m_vars.at(states)(Slice(0, NQ + NU), timeIndices); + mxIn[i + 1] = + m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); } else if (inputs[i] == slacks) { - mxIn[i + 1] = m_vars.at(inputs[i]); + mxIn[i + 1] = m_unscaledVars.at(inputs[i]); } else { - mxIn[i + 1] = m_vars.at(inputs[i])(Slice(), timeIndices); + mxIn[i + 1] = m_unscaledVars.at(inputs[i])(Slice(), timeIndices); } } if (&timeIndices == &m_gridIndices) { diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h index 3baa64f87..cb8129fdf 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -96,8 +96,7 @@ class Transcription { } } - template - struct Constraints { + template struct Constraints { T defects; T residuals; T kinematic; @@ -123,12 +122,15 @@ class Transcription { casadi::MX m_duration; private: - VariablesMX m_vars; + VariablesMX m_scaledVars; + VariablesMX m_unscaledVars; casadi::MX m_paramsTrajGrid; casadi::MX m_paramsTraj; casadi::MX m_paramsTrajIgnoringConstraints; VariablesDM m_lowerBounds; VariablesDM m_upperBounds; + VariablesDM m_shift; + VariablesDM m_scale; casadi::DM m_kinematicConstraintIndices; casadi::Matrix m_gridIndices; @@ -166,11 +168,11 @@ class Transcription { void transcribe(); void setObjective(); void calcDefects() { - calcDefectsImpl(m_vars.at(states), m_xdot, m_constraints.defects); + calcDefectsImpl(m_unscaledVars.at(states), m_xdot, m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( - m_vars.at(controls), m_constraints.interp_controls); + m_unscaledVars.at(controls), m_constraints.interp_controls); } /// Use this function to ensure you iterate through variables in the same @@ -193,12 +195,12 @@ class Transcription { return T::veccat(stdvec); } /// Convert the 'x' column vector into separate variables. - CasOC::VariablesDM expandVariables(const casadi::DM& x) const { - CasOC::VariablesDM out; + VariablesDM expandVariables(const casadi::DM& x) const { + VariablesDM out; using casadi::Slice; casadi_int offset = 0; - for (const auto& key : getSortedVarKeys(m_vars)) { - const auto& value = m_vars.at(key); + for (const auto& key : getSortedVarKeys(m_scaledVars)) { + const auto& value = m_scaledVars.at(key); // Convert a portion of the column vector into a matrix. out[key] = casadi::DM::reshape( x(Slice(offset, offset + value.numel())), value.rows(), @@ -208,6 +210,36 @@ class Transcription { return out; } + /// unscaled = (upper - lower) * scaled - 0.5 * (upper + lower); + template + Variables unscaleVariables(const Variables& scaledVars) { + Variables out; + + for (const auto& kv : scaledVars) { + const auto& key = kv.first; + const auto& scaled = scaledVars.at(key); + const auto& shift = m_shift.at(key); + const auto& scale = m_scale.at(key); + out[key] = scaled * scale + shift; + } + return out; + } + + // TODO scale/unscale guess! + template + Variables scaleVariables(const Variables& unscaledVars) { + Variables out; + + for (const auto& kv : unscaledVars) { + const auto& key = kv.first; + const auto& unscaled = unscaledVars.at(key); + const auto& shift = m_shift.at(key); + const auto& scale = m_scale.at(key); + out[key] = (unscaled - shift) / scale; + } + return out; + } + /// Flatten the constraints into a row vector, keeping constraints /// grouped together by time. Organizing the sparsity of the Jacobian /// this way might have benefits for sparse linear algebra. diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index b5bed3bc9..d431e2c9f 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -35,6 +35,7 @@ using namespace OpenSim; MocoCasADiSolver::MocoCasADiSolver() { constructProperties(); } void MocoCasADiSolver::constructProperties() { + constructProperty_scale_variables_using_bounds(false); constructProperty_parameters_require_initsystem(true); constructProperty_optim_sparsity_detection("none"); constructProperty_optim_write_sparsity(""); @@ -259,6 +260,7 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( casSolver->setMesh(mesh); } casSolver->setTranscriptionScheme(get_transcription_scheme()); + casSolver->setScaleVariablesUsingBounds(get_scale_variables_using_bounds()); casSolver->setMinimizeLagrangeMultipliers( get_minimize_lagrange_multipliers()); casSolver->setLagrangeMultiplierWeight(get_lagrange_multiplier_weight()); diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.h b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.h index aeee3b1ff..5207eb0dd 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.h +++ b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.h @@ -112,6 +112,8 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { MocoCasADiSolver, MocoDirectCollocationSolver); public: + OpenSim_DECLARE_PROPERTY(scale_variables_using_bounds, bool, + "TODO"); OpenSim_DECLARE_PROPERTY(parameters_require_initsystem, bool, "Do some MocoParameters in the problem require invoking " "initSystem() to take effect properly? " diff --git a/Moco/Moco/MocoProblem.cpp b/Moco/Moco/MocoProblem.cpp index bcb5b91f1..81a44bbf9 100644 --- a/Moco/Moco/MocoProblem.cpp +++ b/Moco/Moco/MocoProblem.cpp @@ -30,7 +30,7 @@ void MocoPhase::constructProperties() { constructProperty_model(ModelProcessor(Model{})); constructProperty_time_initial_bounds(MocoInitialBounds()); constructProperty_time_final_bounds(MocoFinalBounds()); - constructProperty_default_speed_bounds(MocoBounds(-50, 50)); + constructProperty_default_speed_bounds(MocoBounds(-20, 20)); constructProperty_bound_activation_from_excitation(true); constructProperty_state_infos(); constructProperty_state_infos_pattern(); diff --git a/tropter/tropter/optimization/AbstractProblem.h b/tropter/tropter/optimization/AbstractProblem.h index 0197b4965..c999d9edd 100644 --- a/tropter/tropter/optimization/AbstractProblem.h +++ b/tropter/tropter/optimization/AbstractProblem.h @@ -152,8 +152,10 @@ class AbstractProblem { m_variable_upper_bounds = upper; } + void set_variable_scaling_from_range( const Eigen::VectorXd& lower, const Eigen::VectorXd& upper) { + // TODO special handling if upper == lower!!! // Betts 2010 page 167. set_variable_scaling(1.0 / (upper.array() - lower.array()), 0.5 - upper.array() / (upper.array() - lower.array())); From c8778900bdf2d6d6f918b81627d3e7bd350d6bc9 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 12 Jul 2019 15:07:14 -0700 Subject: [PATCH 3/5] Scale defects in CasADi. --- .../MocoCasADiSolver/CasOCTranscription.cpp | 93 +++++++++---------- .../MocoCasADiSolver/CasOCTranscription.h | 56 ++++++++++- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 2 +- 3 files changed, 97 insertions(+), 54 deletions(-) diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index c5d81ccd3..c30840dc5 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -21,6 +21,7 @@ using casadi::DM; using casadi::MX; using casadi::MXVector; using casadi::Slice; +using casadi::Sparsity; namespace CasOC { @@ -154,19 +155,32 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_daeIndicesIgnoringConstraints = makeTimeIndices(daeIndicesIgnoringConstraintsVector); - // Set variable bounds. - // -------------------- - auto initializeVariablesDM = [&](VariablesDM& bounds) { + // Set variable bounds and scale factors. + // -------------------------------------- + auto initializeBoundsDM = [&](VariablesDM& bounds) { for (auto& kv : m_scaledVars) { - bounds[kv.first] = DM(kv.second.rows(), kv.second.columns()); + bounds[kv.first] = + DM(Sparsity::dense(kv.second.rows(), kv.second.columns())); } }; - initializeVariablesDM(m_lowerBounds); - initializeVariablesDM(m_upperBounds); + initializeBoundsDM(m_lowerBounds); + initializeBoundsDM(m_upperBounds); + + // The VariablesDM for scaling have length 1 in the time dimension. + auto initializeScalingDM = [&](VariablesDM& bounds) { + for (auto& kv : m_scaledVars) { + bounds[kv.first] = DM(Sparsity::dense(kv.second.rows(), 1)); + } + }; + initializeScalingDM(m_shift); + initializeScalingDM(m_scale); setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); + setScalingUsingBounds(initial_time, m_problem.getTimeInitialBounds()); + setScalingUsingBounds(final_time, m_problem.getTimeFinalBounds()); + { const auto& stateInfos = m_problem.getStateInfos(); int is = 0; @@ -177,6 +191,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(states, is, 0, info.initialBounds); // The "-1" grabs the last column (last mesh point). setVariableBounds(states, is, -1, info.finalBounds); + + setScalingUsingBounds(states, info.bounds); + ++is; } } @@ -188,6 +205,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, controls, ic, Slice(1, m_numGridPoints - 1), info.bounds); setVariableBounds(controls, ic, 0, info.initialBounds); setVariableBounds(controls, ic, -1, info.finalBounds); + + setScalingUsingBounds(controls, info.bounds); + ++ic; } } @@ -199,6 +219,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, info.bounds); setVariableBounds(multipliers, im, 0, info.initialBounds); setVariableBounds(multipliers, im, -1, info.finalBounds); + + setScalingUsingBounds(multipliers, info.bounds); + ++im; } } @@ -207,7 +230,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // "Slice()" grabs everything in that dimension (like ":" in // Matlab). // TODO: How to choose bounds on udot? - setVariableBounds(derivatives, Slice(), Slice(), {-1000, 1000}); + Bounds bounds(-1000.0, 1000.0); + setVariableBounds(derivatives, Slice(), Slice(), bounds); + + setScalingUsingBounds(derivatives, bounds); } } { @@ -215,6 +241,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int isl = 0; for (const auto& info : slackInfos) { setVariableBounds(slacks, isl, Slice(), info.bounds); + + setScalingUsingBounds(slacks, info.bounds); ++isl; } } @@ -223,44 +251,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int ip = 0; for (const auto& info : paramInfos) { setVariableBounds(parameters, ip, 0, info.bounds); - ++ip; - } - } - // Handle scaling of variables. - // ---------------------------- - initializeVariablesDM(m_shift); - initializeVariablesDM(m_scale); - - for (const auto& kv : m_scaledVars) { - const auto& key = kv.first; - // TODO: do NOT use initial/final bounds for scaling; always use phase - // bounds. - if (m_solver.getScaleVariablesUsingBounds()) { - const auto& L = m_lowerBounds.at(key); - const auto& U = m_upperBounds.at(key); - for (int irow = 0; irow < kv.second.rows(); ++irow) { - for (int icol = 0; icol < kv.second.columns(); ++icol) { - const double upper = U(irow, icol).scalar(); - const double lower = L(irow, icol).scalar(); - double range = upper - lower; - double midpoint; - if (std::isinf(range)) { - range = 1; - midpoint = 0; - } else if (range == 0) { - range = 1; - midpoint = upper; - } else { - midpoint = -0.5 * (upper + lower); - } - m_scale.at(key)(irow, icol) = range; - m_shift.at(key)(irow, icol) = midpoint; - } - } - } else { - m_scale.at(key) = 1; - m_shift.at(key) = 0; + setScalingUsingBounds(parameters, info.bounds); + ++ip; } } @@ -328,8 +321,10 @@ void Transcription::transcribe() { // qdot // ---- - const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); - m_xdot(Slice(0, NQ), Slice()) = u; + { + const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); + m_xdot(Slice(0, NQ), Slice()) = u; + } if (m_problem.getEnforceConstraintDerivatives() && m_numPointsIgnoringConstraints) { @@ -354,8 +349,10 @@ void Transcription::transcribe() { if (m_solver.isDynamicsModeImplicit()) { // udot. - const MX w = m_unscaledVars[derivatives]; - m_xdot(Slice(NQ, NQ + NU), Slice()) = w; + { + const MX w = m_unscaledVars[derivatives]; + m_xdot(Slice(NQ, NQ + NU), Slice()) = w; + } std::vector inputs{states, controls, multipliers, derivatives}; diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h index cb8129fdf..8793aabea 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -96,6 +96,29 @@ class Transcription { } } + void setScalingUsingBounds(Var key, const Bounds& bounds) { + if (m_solver.getScaleVariablesUsingBounds()) { + const auto& lower = bounds.lower; + const auto& upper = bounds.upper; + double range = upper - lower; + double midpoint; + if (std::isinf(range)) { + range = 1; + midpoint = 0; + } else if (range == 0) { + range = 1; + midpoint = upper; + } else { + midpoint = -0.5 * (upper + lower); + } + m_scale.at(key)(casadi::Slice()) = range; + m_shift.at(key)(casadi::Slice()) = midpoint; + } else { + m_scale.at(key)(casadi::Slice()) = 1; + m_shift.at(key)(casadi::Slice()) = 0; + } + } + template struct Constraints { T defects; T residuals; @@ -167,11 +190,24 @@ class Transcription { void transcribe(); void setObjective(); + // TODO document about scaling. void calcDefects() { - calcDefectsImpl(m_unscaledVars.at(states), m_xdot, m_constraints.defects); + calcDefectsImpl( + m_unscaledVars.at(states), m_xdot, m_constraints.defects); + if (m_solver.getScaleVariablesUsingBounds() && + m_problem.getNumStates()) { + // TODO this is hacky: + const auto repRow = + m_constraints.defects.rows() / m_problem.getNumStates(); + const auto repCol = m_constraints.defects.columns(); + m_constraints.defects /= + casadi::DM::repmat(m_scale.at(states), repRow, repCol); + } } + // TODO document about scaling. void calcInterpolatingControls() { calcInterpolatingControlsImpl( + // TODO can use scaled controls! m_unscaledVars.at(controls), m_constraints.interp_controls); } @@ -213,13 +249,18 @@ class Transcription { /// unscaled = (upper - lower) * scaled - 0.5 * (upper + lower); template Variables unscaleVariables(const Variables& scaledVars) { + using casadi::DM; Variables out; for (const auto& kv : scaledVars) { const auto& key = kv.first; const auto& scaled = scaledVars.at(key); - const auto& shift = m_shift.at(key); - const auto& scale = m_scale.at(key); + const auto& numCols = scaled.columns(); + // The shift and scale are column vectors. For appropriate + // elementwise math, we repeat the column to match the number of + // columns for this key. + const auto& shift = DM::repmat(m_shift.at(key), 1, numCols); + const auto& scale = DM::repmat(m_scale.at(key), 1, numCols); out[key] = scaled * scale + shift; } return out; @@ -228,13 +269,18 @@ class Transcription { // TODO scale/unscale guess! template Variables scaleVariables(const Variables& unscaledVars) { + using casadi::DM; Variables out; for (const auto& kv : unscaledVars) { const auto& key = kv.first; const auto& unscaled = unscaledVars.at(key); - const auto& shift = m_shift.at(key); - const auto& scale = m_scale.at(key); + const auto& numCols = unscaled.columns(); + // The shift and scale are column vectors. For appropriate + // elementwise math, we repeat the column to match the number of + // columns for this key. + const auto& shift = DM::repmat(m_shift.at(key), 1, numCols); + const auto& scale = DM::repmat(m_scale.at(key), 1, numCols); out[key] = (unscaled - shift) / scale; } return out; diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index d431e2c9f..6eff7426a 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -35,7 +35,7 @@ using namespace OpenSim; MocoCasADiSolver::MocoCasADiSolver() { constructProperties(); } void MocoCasADiSolver::constructProperties() { - constructProperty_scale_variables_using_bounds(false); + constructProperty_scale_variables_using_bounds(true); constructProperty_parameters_require_initsystem(true); constructProperty_optim_sparsity_detection("none"); constructProperty_optim_write_sparsity(""); From b091119596a8b6b11b71181840aa9a972cfe7c5e Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 12 Jul 2019 18:41:09 -0700 Subject: [PATCH 4/5] Refining variable scaling. --- .../MocoCasADiSolver/CasOCTranscription.cpp | 13 ++++++----- .../MocoCasADiSolver/CasOCTranscription.h | 22 +++++++++---------- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 2 +- Moco/Tests/testMocoInterface.cpp | 12 +++++----- 4 files changed, 26 insertions(+), 23 deletions(-) diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 5847d785b..9701a651a 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -457,7 +457,8 @@ void Transcription::setObjective() { // Minimize Lagrange multipliers if specified by the solver. if (m_solver.getMinimizeLagrangeMultipliers() && m_problem.getNumMultipliers()) { - const auto mults = m_vars[multipliers]; + // TODO: should we scale? + const auto mults = m_scaledVars[multipliers]; const double multiplierWeight = m_solver.getLagrangeMultiplierWeight(); // Sum across constraints of each multiplier element squared. MX integrandTraj = multiplierWeight * MX::sum1(MX::sq(mults)); @@ -486,15 +487,17 @@ void Transcription::setObjective() { MXVector costOut; info.cost_function->call( - {m_unscaledVars[initial_time], m_unscaledVars[states](Slice(), 0), + {m_unscaledVars[initial_time], + m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), - m_unscaledVars[derivatives](Slice(), 0), m_unscaledVars[final_time], + m_unscaledVars[derivatives](Slice(), 0), + m_unscaledVars[final_time], m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), - m_unscaledVars[derivatives](Slice(), -1), m_unscaledVars[parameters], - integral}, + m_unscaledVars[derivatives](Slice(), -1), + m_unscaledVars[parameters], integral}, costOut); m_objective += costOut.at(0); } diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h index 8793aabea..66dca8d21 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -100,19 +100,19 @@ class Transcription { if (m_solver.getScaleVariablesUsingBounds()) { const auto& lower = bounds.lower; const auto& upper = bounds.upper; - double range = upper - lower; - double midpoint; - if (std::isinf(range)) { - range = 1; - midpoint = 0; - } else if (range == 0) { - range = 1; - midpoint = upper; + double dilate = upper - lower; + double shift; + if (std::isinf(dilate) || std::isnan(dilate)) { + dilate = 1; + shift = 0; + } else if (dilate == 0) { + dilate = 1; + shift = upper; } else { - midpoint = -0.5 * (upper + lower); + shift = -0.5 * (upper + lower); } - m_scale.at(key)(casadi::Slice()) = range; - m_shift.at(key)(casadi::Slice()) = midpoint; + m_scale.at(key)(casadi::Slice()) = dilate; + m_shift.at(key)(casadi::Slice()) = shift; } else { m_scale.at(key)(casadi::Slice()) = 1; m_shift.at(key)(casadi::Slice()) = 0; diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 6eff7426a..d431e2c9f 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/Moco/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -35,7 +35,7 @@ using namespace OpenSim; MocoCasADiSolver::MocoCasADiSolver() { constructProperties(); } void MocoCasADiSolver::constructProperties() { - constructProperty_scale_variables_using_bounds(true); + constructProperty_scale_variables_using_bounds(false); constructProperty_parameters_require_initsystem(true); constructProperty_optim_sparsity_detection("none"); constructProperty_optim_write_sparsity(""); diff --git a/Moco/Tests/testMocoInterface.cpp b/Moco/Tests/testMocoInterface.cpp index b76ac5ffa..637f31e77 100644 --- a/Moco/Tests/testMocoInterface.cpp +++ b/Moco/Tests/testMocoInterface.cpp @@ -462,8 +462,8 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { // Default speed bounds. const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getBounds().getLower(), -20); + SimTK_TEST_EQ(info.getBounds().getUpper(), 20); } // No control info stored in the Problem. SimTK_TEST_MUST_THROW_EXC( @@ -563,8 +563,8 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getBounds().getLower(), -20); + SimTK_TEST_EQ(info.getBounds().getUpper(), 20); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -4.1); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.9); SimTK_TEST(!info.getFinalBounds().isSet()); @@ -582,8 +582,8 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getBounds().getLower(), -20); + SimTK_TEST_EQ(info.getBounds().getUpper(), 20); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 0.1); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 0.8); From 9a45531f04e0d9966900a138e19dad951b04f199 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 12 Jul 2019 18:54:09 -0700 Subject: [PATCH 5/5] Revert speed bounds to [-50, 50] so testMocoTrack passes. --- Moco/Moco/MocoProblem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Moco/Moco/MocoProblem.cpp b/Moco/Moco/MocoProblem.cpp index 81a44bbf9..bcb5b91f1 100644 --- a/Moco/Moco/MocoProblem.cpp +++ b/Moco/Moco/MocoProblem.cpp @@ -30,7 +30,7 @@ void MocoPhase::constructProperties() { constructProperty_model(ModelProcessor(Model{})); constructProperty_time_initial_bounds(MocoInitialBounds()); constructProperty_time_final_bounds(MocoFinalBounds()); - constructProperty_default_speed_bounds(MocoBounds(-20, 20)); + constructProperty_default_speed_bounds(MocoBounds(-50, 50)); constructProperty_bound_activation_from_excitation(true); constructProperty_state_infos(); constructProperty_state_infos_pattern();