Support for Model Constraints

[nickbianco]

Let's get constraints in Muscollo! Here's a rough, high-level outline to get us started. Feel free to make any changes or additions.

The Simbody method for providing a unique solution to the Lagrange multipliers is shown below. It involves differentiating position and velocity-level constraint equations to get acceleration level constraint equations which can be restricted by Simbody when evaluating the dynamics.

However, to as a starting point, we'll be going with a suggestion by Ton, where we only solve the position level (or velocity level) constraint equations and minimize the Lagrange multipliers to get a unique solution. This may not be an ideal long-term solution, since minimizing Lagrange multipliers is the same as minimizing reaction loads, which isn't necessarily a desired outcome. However, it may be suitable to get a simple model including constraints working first (e.g. the hopper). We can sort out the bugs there, and then try a more appropriate formulation later on.

Here is the problem we will be solving. The contributions from constraints is shown as applied constraint forces here, since that is analogous to how Simbody will be utilized when formulating the dynamics.

Below is a checklist of the necessary changes/additions to Muscollo, more or less in order of planned implementation.

Sandbox for testing changes to calculations of DAE's

• [x] createModel() -- This creates a toy OpenSim model to use in the tests.
• [x] createState() -- Create a common random state so that the Simbody constraint results and our custom implementation for calculating dynamics can be compared.
• [x] getAccelerationsFromMultipliers(model, state, multipliers, udot) -- The custom function to get accelerations from the Lagrange multipliers, which will eventually be variables in a direct collocation problem. The following Simbody calls will be made:
  • [x] multibodySystem = model.getMultibodySystem()
  • [x] appliedBodyForces = multibodySystem.getRigidBodyForces(state, Stage::Dynamics)
  • [x] appliedMobilityForces = multibodySystem.getMobilityForces(state, Stage::Dynamics)
  • [x] system.calcConstraintForcesFromMultipliers(state, -multipliers, constraintBodyForces, constraintMobilityForces) to get constraint body and mobility forces. Note that the multipliers are negated so that constraint forces can be used like applied forces.
  • [x] system.calcAccelerationIgnoringConstraints(state, appliedMobilityForces+constraintMobilityForces, appliedBodyForces+constraintBodyForces, udot, A_GB)

Sandbox test cases will follow a similar formula to Simbody's TestConstraints.cpp: 1) model = createModel() 2) Impose constraint to test 3) state = createState() 3) uDot_Simbody = system.getUDot(state) 4) multipliers = system.getMultipliers(state) 5) uDot_Custom = getAccelerationsFromMultipliers(model, state, multipliers, udot) 6) Compare uDot_Simbody and uDot_Custom

Sandbox test cases

• [x] WeldConstraint
• [x] PointConstraint
• [x] PointOnLineConstraint
• [x] ConstantDistance
• [x] Others?

Direct collocation test cases

• [x] Test 1 - Optimize torque control for a simple double inverted pendulum model to reach a desired end-effector height, with a PointOnLineConstraint imposed on the end-effector.
• [x] Test 2 - Using the same double inverted pendulum, optimize to reach desired end-effector height, now using a CoordinateCouplerConstraint and only one torque control.
• [x] Test 3 - Using the same double inverted pendulum, set a PrescribedMotion constraint using result from previous test. Check that multipliers match torque/multiplier from previous test.

MucoTropterSolver.cpp

• [x] Add ability to introduce control variables not associated to the OpenSim model controls (similar to this implementation in branch contact_tracking_misc)
• [x] Get Simbody matter subsystem from model (Model::getMatterSubsystem())
• [x] For each Simbody constraint (get indices from SimbodyMatterSubsystem::getNumConstraints()):
  • [x] Add a tropter path constraint
  • [x] Add the associated Lagrange multiplier as a tropter control variable
• [x] Add squared multiplier cost to integrand in calc_integral_cost()
• [x] Add changes to DAE calculations (i.e. getAccelerationsFromMultipliers(model, state, multipliers, udot), etc.)

Things to finish before merging to master

• [x] Proper detection of Lagrange multipliers compared to normal controls
• [x] Method to properly (automatically?) set Lagrange multiplier info
• [x] Everything in #149

[chrisdembia]

Very thorough!

state = createState() uDot_Simbody = system.getUDot(state) multipliers = system.getMultipliers(state)

I think you need to realize to acceleration after createState().

Others?

It might be easy enough to test prescribed motion and locking a coordinate as part of the DAE tests, and we could catch errors we might encounter later on in the direct collocation tests.

Direct collocation test cases

I like these test cases.

What are you thinking of testing to ensure the constraints are obeyed? You could do a forward simulation with the optimized control and ensure the states trajectory matches. It would be good to check that the multipliers make sense , not sure how exactly.

Check that multipliers match torque/multiplier from previous test.

Seems like MucoIterate wil need to be able to provide the multipliers.

MucoTropterSolver.cpp

In a way, I like how the changes do not leak into the Muscollo interface, which we had discussed. But I'm realizing that it might make sense to leak some stuff through the interface, like potentially the weight on the penalty term or accessing multipliers in MucoIterate.

[nickbianco]

I planned to have createState() realize to acceleration, as it does in Simbody's TestConstraints.cpp. Unless there's reasons to do that separately.

It might be easy enough to test prescribed motion and locking a coordinate as part of the DAE tests, and we could catch errors we might encounter later on in the direct collocation tests.

I'll try adding these to the DAE tests then.

What are you thinking of testing to ensure the constraints are obeyed? You could do a forward simulation with the optimized control and ensure the states trajectory matches.

That's one option. In some cases, like the PointOnLineConstraint, we know that the end-effector won't deviate from the constraint's in-plane position, so we could check that directly.

It would be good to check that the multipliers make sense , not sure how exactly.

This is less clear to me. For the CoordinateCouplerConstraint, I would expect the actual torque and the constraint torque to be within an order of magnitude with each other, although that's not very precise. We might need to go on a case-by-case basis.

Seems like MucoIterate will need to be able to provide the multipliers.

If the multipliers are being set as tropter controls for now, as long as I give them unique names and bounds, it looks like they already should show up in the MucoSolution, according to MucoTropterSolver::solveImpl(). It should happen here, right?

In a way, I like how the changes do not leak into the Muscollo interface, which we had discussed. But I'm realizing that it might make sense to leak some stuff through the interface, like potentially the weight on the penalty term or accessing multipliers in MucoIterate.

They could be temporary changes for now, until we find a way to hide them from the user. I do like the idea of being able to access multipliers or other "support" variables though, maybe through an interface option. Hopefully penalty term weight is temporary anyway, if we can get the Simbody/Michael Posa constraint formulation working later.

[chrisdembia]

I planned to have createState() realize to acceleration, as it does in Simbody's TestConstraints.cpp.

Sounds good.

If the multipliers are being set as tropter controls for now, as long as I give them unique names and bounds, it looks like they already should show up in the MucoSolution, according to MucoTropterSolver::solveImpl(). It should happen here, right?

Oh good point. Down the road, I would prefer that MucoSolution's "controls" would be the model's controls (for actuators), and would not include fiber velocity or multipliers (seems like we're on the same page). But we can deal with that later.

Hopefully penalty term weight is temporary anyway, if we can get the Simbody/Michael Posa constraint formulation working later.

Agreed.

[chrisdembia]

@nickbianco I just got a Google Scholar notification about a masters thesis from McPhee's lab on optimal control of a bicycle using GPOPS-II. The bicycle is a closed kinematic chain, so the the authors had to add kinematics constraints in their GPOPS-II problem.

https://uwspace.uwaterloo.ca/bitstream/handle/10012/13733/Jansen_Conor.pdf?sequence=3&isAllowed=y

[nickbianco]

Interesting. They say they computed reactions forces directly as state variables, and included the reaction force derivatives as controls. I'm not sure if the Lagrange multipliers we're computed at all.

[nickbianco]

@chrisdembia I went through #149 and everything was covered, so I closed it. I don't think there's anything else to add to this checklist, so I'm good to close this when you are.

[chrisdembia]

:) Great work, @nickbianco