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.

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

[WIP] Add FATROP solver support to Moco #3906

Closed
wants to merge 41 commits into from
Closed
Changes from 1 commit
Commits
Show all changes
41 commits
Select commit Hold shift + click to select a range
439eb31
Create time variables across the trajectory
nickbianco Jul 26, 2024
189caa2
Update createTimes()
nickbianco Jul 29, 2024
c761f25
Time constraints now working
nickbianco Jul 29, 2024
9be0783
Removing commented out lines for time changes
nickbianco Jul 29, 2024
d3c56ff
Start adding support for symbolically interpolated controls
nickbianco Aug 2, 2024
f89e50a
Define parameters at all time points
nickbianco Aug 6, 2024
ec111ed
Testing with current control interpolation
nickbianco Aug 6, 2024
0d3946c
Treat time variables as parameters
nickbianco Aug 7, 2024
9a5a29d
First basic problem working (!)
nickbianco Aug 7, 2024
f47d6bf
Remove unneeded code for time and parameter constraints
nickbianco Aug 7, 2024
c5041ab
Provide interface for variable ordering in transcription schemes
nickbianco Aug 9, 2024
e468126
Add time and parameter defects to all transcription schemes
nickbianco Aug 9, 2024
af3dd35
Fix variable ordering for trapezoidal rule
nickbianco Aug 9, 2024
f47938f
Clean up parameter repmatting
nickbianco Aug 9, 2024
b84b58e
Start implementing symbolic control interpolation
nickbianco Aug 12, 2024
d073e49
Problem solving with interpolated controls!
nickbianco Aug 12, 2024
f7650cf
Problem is working with controls (!)
nickbianco Aug 14, 2024
c05a28e
Apply unscaleVariables and calcInterpolatingControls to callback func…
nickbianco Aug 14, 2024
5b6105d
Some cleanup and helpful comments
nickbianco Aug 15, 2024
edb4919
Fix mesh interval calculation
nickbianco Aug 15, 2024
4fd291b
Fixing sparsity for CasOCLegendreGaussRadau
nickbianco Aug 15, 2024
7b4190e
Actually fix sparsity for LegendreGaussRadau
nickbianco Aug 16, 2024
f9d4071
Start adding support for endpoint constraints in FATROP
nickbianco Aug 19, 2024
9a52a22
Merge branch 'main' into moco_fatrop_with_bordalba
nickbianco Aug 26, 2024
a9616f4
Resolve build errors
nickbianco Aug 26, 2024
5329b2d
Still working on support for endpoint constraints in FATROP
nickbianco Aug 29, 2024
b9601a6
Making solving kinematic constraints compatible with FATROP
nickbianco Sep 6, 2024
e02eb3c
Add helper to CasOCTranscription for packing states and state derivat…
nickbianco Sep 9, 2024
9084ddd
Add FATROP support for problems with kinematic constraints; refactor …
nickbianco Sep 11, 2024
232f48a
Merge branch 'main' into moco_fatrop
nickbianco Sep 11, 2024
8f74b2a
Revert exampleSlidingMass
nickbianco Sep 11, 2024
337d2ff
Remove control interpolation and midpoint path constraint options in …
nickbianco Sep 11, 2024
a940c2c
Fixing more tests
nickbianco Sep 12, 2024
b67ee60
Fixes for testMocoGoals, testMocoActuators, and testMocoImplicit
nickbianco Sep 12, 2024
d3d86d4
Use initial and final indices for endpoint function calls in CasOCTra…
nickbianco Sep 12, 2024
66411e1
Fix last set of failing tests
nickbianco Sep 13, 2024
dae3a81
Add CMake build flag for FATROP
nickbianco Sep 13, 2024
151fed0
Silence stdext::checked_array_iterator warnings
nickbianco Sep 13, 2024
d860f87
Remove control interpolation option from MocoCasADiSolver
nickbianco Sep 13, 2024
8526ebb
Add FATROP tests
nickbianco Sep 17, 2024
93d0060
Merge branch 'silence_checked_array_iterator_warning' into moco_fatrop
nickbianco Sep 17, 2024
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
Prev Previous commit
Next Next commit
Still working on support for endpoint constraints in FATROP
  • Loading branch information
nickbianco committed Aug 29, 2024
commit 5329b2d47c058677ad0aa49c9c69299b05387cf0
12 changes: 7 additions & 5 deletions OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp
Original file line number Diff line number Diff line change
@@ -99,15 +99,17 @@ int main() {
// -----
problem.addGoal<MocoFinalTimeGoal>();

auto* endpoint = problem.addGoal<MocoOutputGoal>("endpoint");
endpoint->setOutputPath("/body|linear_velocity");
endpoint->setMode("endpoint_constraint");
// auto* vel = problem.addGoal<MocoOutputGoal>("velocity_integral_bound");
// vel->setOutputPath("/body|linear_velocity");
// vel->setMode("endpoint_constraint");
// vel->setEndpointConstraintBounds({{-250, 250}});

// Configure the solver.
// =====================
MocoCasADiSolver& solver = study.initCasADiSolver();
solver.set_num_mesh_intervals(50);
solver.set_parallel(0);
solver.set_num_mesh_intervals(20);
solver.set_optim_finite_difference_scheme("forward");
// solver.set_parallel(0);
solver.set_optim_solver("fatrop");
solver.set_optim_hessian_approximation("exact");
solver.set_transcription_scheme("legendre-gauss-3");
15 changes: 3 additions & 12 deletions OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp
Original file line number Diff line number Diff line change
@@ -57,31 +57,22 @@ DM HermiteSimpson::createControlIndicesImpl() const {
}

void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti, const casadi::MX& tf,
const casadi::MX& p, casadi::MX& defects) const {
const casadi::MXVector& xdot, casadi::MX& defects) const {
// For more information, see doxygen documentation for the class.

const int NS = m_problem.getNumStates();
const int NP = m_problem.getNumParameters();
for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) {
// We enforce defect constraints on a mesh interval basis, so add
// constraints until the number of mesh intervals is reached.
const auto h = m_times(2 * imesh + 2) - m_times(2 * imesh);
const auto h = m_intervals(imesh);
const auto x_i = x[imesh](Slice(), 0);
const auto x_mid = x[imesh](Slice(), 1);
const auto x_ip1 = x[imesh](Slice(), 2);
const auto xdot_i = xdot[imesh](Slice(), 0);
const auto xdot_mid = xdot[imesh](Slice(), 1);
const auto xdot_ip1 = xdot[imesh](Slice(), 2);

// Time variables.
defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh);
defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh);

// Parameters.
defects(Slice(2, 2 + NP), imesh) =
p(Slice(), imesh + 1) - p(Slice(), imesh);

// Hermite interpolant defects.
defects(Slice(2 + NP, 2 + NP + NS), imesh) =
x_mid - 0.5 * (x_ip1 + x_i) - (h / 8.0) * (xdot_i - xdot_ip1);
@@ -110,7 +101,7 @@ std::vector<std::pair<Var, int>> HermiteSimpson::getVariableOrder() const {
order.push_back({parameters, imesh});
order.push_back({states, igrid});
order.push_back({states, igrid + 1});
if (m_solver.getInterpolateControlMidpoints()) {
if (m_solver.getInterpolateControlMeshInteriorPoints()) {
order.push_back({controls, igrid});
} else {
order.push_back({controls, igrid});
4 changes: 1 addition & 3 deletions OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h
Original file line number Diff line number Diff line change
@@ -75,9 +75,7 @@ class HermiteSimpson : public Transcription {
casadi::DM createMeshIndicesImpl() const override;
casadi::DM createControlIndicesImpl() const override;
void calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti,
const casadi::MX& tf, const casadi::MX& p,
casadi::MX& defects) const override;
const casadi::MXVector& xdot, casadi::MX& defects) const override;
std::vector<std::pair<Var, int>> getVariableOrder() const override;
void calcInterpolatingControlsImpl(casadi::MX& controls) const override;
void calcInterpolatingControlsImpl(casadi::DM& controls) const override;
4 changes: 2 additions & 2 deletions OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h
Original file line number Diff line number Diff line change
@@ -43,8 +43,8 @@ enum Var {
/// A "mirror" of the multibody states used in the projection method
/// for solving kinematic constraints.
projection_states,
/// Variables for endpoint constraints.
endpoints,
/// Variables for integral constraints.
integrals,
/// For internal use (never actually a key for Variables).
multibody_states = 100
};
28 changes: 10 additions & 18 deletions OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp
Original file line number Diff line number Diff line change
@@ -66,16 +66,14 @@ DM LegendreGauss::createControlIndicesImpl() const {
}

void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti,
const casadi::MX& tf, const casadi::MX& p,
casadi::MX& defects) const {
const casadi::MXVector& xdot, casadi::MX& defects) const {
// For more information, see doxygen documentation for the class.

const int NS = m_problem.getNumStates();
const int NP = m_problem.getNumParameters();
for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) {
const int igrid = imesh * (m_degree + 1);
const auto h = m_times(igrid + m_degree + 1) - m_times(igrid);
const auto h = m_intervals(imesh);
const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1));
const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1));
const auto x_ip1 = x[imesh](Slice(), m_degree + 1);
@@ -84,19 +82,11 @@ void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x,
defects(Slice(0, NS), imesh) =
x_ip1 - MX::mtimes(x_i, m_interpolationCoefficients);

// Time variables.
defects(Slice(NS, NS + 1), imesh) = ti(imesh + 1) - ti(imesh);
defects(Slice(NS + 1, NS + 2), imesh) = tf(imesh + 1) - tf(imesh);

// Parameters.
defects(Slice(NS + 2, NS + 2 + NP), imesh) =
p(Slice(), imesh + 1) - p(Slice(), imesh);

// Residual function defects.
MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix);
for (int d = 0; d < m_degree; ++d) {
const int istart = (d + 1) * NS + 2 + NP;
const int iend = (d + 2) * NS + 2 + NP;
const int istart = (d + 1) * NS;
const int iend = (d + 2) * NS;
defects(Slice(istart, iend), imesh) = residual(Slice(), d);
}
}
@@ -115,14 +105,13 @@ std::vector<std::pair<Var, int>> LegendreGauss::getVariableOrder() const {
int N = m_numPointsPerMeshInterval - 1;
for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) {
int igrid = imesh * N;
order.push_back({states, igrid});
order.push_back({initial_time, imesh});
order.push_back({final_time, imesh});
order.push_back({parameters, imesh});
for (int i = 1; i < N; ++i) {
for (int i = 0; i < N; ++i) {
order.push_back({states, igrid + i});
}
if (m_solver.getInterpolateControlMidpoints()) {
if (m_solver.getInterpolateControlMeshInteriorPoints()) {
for (int d = 0; d < m_degree; ++d) {
order.push_back({controls, igrid + d + 1});
}
@@ -138,12 +127,15 @@ std::vector<std::pair<Var, int>> LegendreGauss::getVariableOrder() const {
order.push_back({derivatives, igrid + i});
}
order.push_back({slacks, imesh});
if (imesh < m_numMeshIntervals - 1) {
order.push_back({integrals, imesh});
}
}
order.push_back({states, m_numGridPoints - 1});
order.push_back({initial_time, m_numMeshIntervals});
order.push_back({final_time, m_numMeshIntervals});
order.push_back({parameters, m_numMeshIntervals});
if (!m_solver.getInterpolateControlMidpoints()) {
if (!m_solver.getInterpolateControlMeshInteriorPoints()) {
order.push_back({controls, m_numGridPoints - 1});
}
order.push_back({multipliers, m_numGridPoints - 1});
4 changes: 1 addition & 3 deletions OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h
Original file line number Diff line number Diff line change
@@ -103,9 +103,7 @@ class LegendreGauss : public Transcription {
casadi::DM createMeshIndicesImpl() const override;
casadi::DM createControlIndicesImpl() const override;
void calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti,
const casadi::MX& tf, const casadi::MX& p,
casadi::MX& defects) const override;
const casadi::MXVector& xdot, casadi::MX& defects) const override;
void calcInterpolatingControlsImpl(casadi::MX& controls) const override;
void calcInterpolatingControlsImpl(casadi::DM& controls) const override;
std::vector<std::pair<Var, int>> getVariableOrder() const override;
18 changes: 4 additions & 14 deletions OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp
Original file line number Diff line number Diff line change
@@ -59,26 +59,17 @@ DM LegendreGaussRadau::createControlIndicesImpl() const {
}

void LegendreGaussRadau::calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti, const casadi::MX& tf,
const casadi::MX& p, casadi::MX& defects) const {
const casadi::MXVector& xdot, casadi::MX& defects) const {
// For more information, see doxygen documentation for the class.

const int NS = m_problem.getNumStates();
const int NP = m_problem.getNumParameters();
for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) {
const int igrid = imesh * m_degree;
const auto h = m_times(igrid + m_degree) - m_times(igrid);
const auto h = m_intervals(imesh);
const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1));
const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1));

// Time variables.
defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh);
defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh);

// Parameters.
defects(Slice(2, 2 + NP), imesh) =
p(Slice(), imesh + 1) - p(Slice(), imesh);

// Residual function defects.
MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix);
for (int d = 0; d < m_degree; ++d) {
@@ -104,14 +95,13 @@ std::vector<std::pair<Var, int>> LegendreGaussRadau::getVariableOrder() const {
int N = m_numPointsPerMeshInterval - 1;
for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) {
int igrid = imesh * N;
order.push_back({states, igrid});
order.push_back({initial_time, imesh});
order.push_back({final_time, imesh});
order.push_back({parameters, imesh});
for (int i = 1; i < N; ++i) {
for (int i = 0; i < N; ++i) {
order.push_back({states, igrid + i});
}
if (m_solver.getInterpolateControlMidpoints() && imesh == 0) {
if (m_solver.getInterpolateControlMeshInteriorPoints() && imesh == 0) {
for (int i = 1; i < N; ++i) {
order.push_back({controls, igrid + i});
}
4 changes: 1 addition & 3 deletions OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h
Original file line number Diff line number Diff line change
@@ -106,9 +106,7 @@ class LegendreGaussRadau : public Transcription {
casadi::DM createMeshIndicesImpl() const override;
casadi::DM createControlIndicesImpl() const override;
void calcDefectsImpl(const casadi::MXVector& x,
const casadi::MXVector& xdot, const casadi::MX& ti,
const casadi::MX& tf, const casadi::MX& p,
casadi::MX& defects) const override;
const casadi::MXVector& xdot, casadi::MX& defects) const override;
void calcInterpolatingControlsImpl(casadi::MX& controls) const override;
void calcInterpolatingControlsImpl(casadi::DM& controls) const override;
std::vector<std::pair<Var, int>> getVariableOrder() const override;
Loading