Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Automatic variable scaling #668

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Automatic variable scaling #668

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8fb5452
Fooling around with scaling in tropter.
chrisdembia Jul 11, 2019
bb64d70
First pass at variable scaling for CasADi. Hinders performance.
chrisdembia Jul 12, 2019
c877890
Scale defects in CasADi.
chrisdembia Jul 12, 2019
1637203
Merge branch 'master' of github.com:stanfordnmbl/opensim-moco into sc…
chrisdembia Jul 12, 2019
b091119
Refining variable scaling.
chrisdembia Jul 13, 2019
9a45531
Revert speed bounds to [-50, 50] so testMocoTrack passes.
chrisdembia Jul 13, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions Moco/Examples/C++/exampleSlidingMass/exampleSlidingMass.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
9 changes: 9 additions & 0 deletions Moco/Moco/MocoCasADiSolver/CasOCSolver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -142,6 +150,7 @@ class Solver {
const Problem& m_problem;
std::vector<double> 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;
Expand Down
140 changes: 92 additions & 48 deletions Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ using casadi::DM;
using casadi::MX;
using casadi::MXVector;
using casadi::Slice;
using casadi::Sparsity;

namespace CasOC {

Expand Down Expand Up @@ -112,28 +113,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<int> daeIndicesVector;
Expand All @@ -160,19 +155,32 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid,
m_daeIndicesIgnoringConstraints =
makeTimeIndices(daeIndicesIgnoringConstraintsVector);

// Set variable bounds.
// --------------------
auto initializeBounds = [&](VariablesDM& bounds) {
for (auto& kv : m_vars) {
bounds[kv.first] = DM(kv.second.rows(), kv.second.columns());
// Set variable bounds and scale factors.
// --------------------------------------
auto initializeBoundsDM = [&](VariablesDM& bounds) {
for (auto& kv : m_scaledVars) {
bounds[kv.first] =
DM(Sparsity::dense(kv.second.rows(), kv.second.columns()));
}
};
initializeBounds(m_lowerBounds);
initializeBounds(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;
Expand All @@ -183,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;
}
}
Expand All @@ -194,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;
}
}
Expand All @@ -205,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;
}
}
Expand All @@ -213,14 +230,19 @@ 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);
}
}
{
const auto& slackInfos = m_problem.getSlackInfos();
int isl = 0;
for (const auto& info : slackInfos) {
setVariableBounds(slacks, isl, Slice(), info.bounds);

setScalingUsingBounds(slacks, info.bounds);
++isl;
}
}
Expand All @@ -229,9 +251,22 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid,
int ip = 0;
for (const auto& info : paramInfos) {
setVariableBounds(parameters, ip, 0, info.bounds);

setScalingUsingBounds(parameters, info.bounds);
++ip;
}
}

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() {
Expand Down Expand Up @@ -286,8 +321,10 @@ void Transcription::transcribe() {

// qdot
// ----
const MX u = m_vars[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) {
Expand All @@ -312,8 +349,10 @@ void Transcription::transcribe() {
if (m_solver.isDynamicsModeImplicit()) {

// udot.
const MX w = m_vars[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<Var> inputs{states, controls, multipliers, derivatives};

Expand Down Expand Up @@ -418,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));
Expand Down Expand Up @@ -447,21 +487,24 @@ void Transcription::setObjective() {
MXVector costOut;

info.cost_function->call(
{m_vars[initial_time], m_vars[states](Slice(), 0),
m_vars[controls](Slice(), 0),
m_vars[multipliers](Slice(), 0),
m_vars[derivatives](Slice(), 0), 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],
integral},
{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[states](Slice(), -1),
m_unscaledVars[controls](Slice(), -1),
m_unscaledVars[multipliers](Slice(), -1),
m_unscaledVars[derivatives](Slice(), -1),
m_unscaledVars[parameters], integral},
costOut);
m_objective += costOut.at(0);
}
}

Solution Transcription::solve(const Iterate& guessOrig) {
// TODO scale guess!

// Define the NLP.
// ---------------
Expand Down Expand Up @@ -512,7 +555,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
Expand Down Expand Up @@ -554,18 +597,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<Solution>();
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]);
Expand Down Expand Up @@ -1030,11 +1073,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) {
Expand Down
Loading