Constraints violated when computing state derivatives

[Adrian Lai]

Hi everyone,

I've been trying to solve a basic optimal control problem of a single leg pedal using some custom MATLAB code that calls OpenSim's dynamics engine.

The model is currently closed loop with the hip/pelvis and the crank pinned to the ground and then I applied a weld constraint between two separate pedal segments (to close the loop). Hence, in the GUI, when I rotate the crank, all the other joints move accordingly. It is also torque actuator driven and has 7 DOFs. I then set a cost function to minimise the torque actuator controls and track the crank angle for a single crank cycle. I'm able to get a solution but when I view the results, the weld constraint is not held so the two pedal segments (one welded to the toe and the other pinned to the crank) are separated. Is there a way of making sure the constraints are not violated?

Thanks in advance for your help!

Adrian

[Dimitar Stanev]

Hi,

Do you perform Forward Dynamics or Inverse? OpenSim ignores constraints during Inverse Dynamics. Also make sure that the initial state is consistent with the constraints.

Best

[Adrian Lai]

I guess it’s forward dynamics? I’m solving an optimal control problem outside of OpenSim. I use the OpenSim’s dynamic engine to compute state derivative but I was hoping that the constraints were included in the equations of motion.

Adrian

[Ross Miller]

Hi Adrian,

Could you describe some more details on how you're solving the optimal control problem?
- are you doing forward integration, direct collocation, something else?
- what optimization routine?
- how big are the constraint violations?

All numerical optimization methods (to my knowledge) that allow constraints to be specified will produce solutions that actually violate those constraints, but for "good" solutions with a verified model, the violations should be negligibly small. Usually you can specify the size of the maximum acceptable violation in the optimizer options.

Hope this helps,
Ross

[Adrian Lai]

Hi Ross,

Thanks for the reply.

Could you describe some more details on how you're solving the optimal control problem?
- are you doing forward integration, direct collocation, something else?
- what optimization routine?
- how big are the constraint violations?

I'm doing direct collocation using IPOPT through MATLAB and calling OpenSim to compute my state derivative to obtain my constraint equations and Jacobian. My constraint violations actually converge quite nicely from a random initial guess but I think I didn't describe what I meant by constraints very well.

I'm not talking about the constraints in my optimisation routine but rather the constraints applied to the musculoskeletal model (i.e. system dynamics), which you set in the model (.osim) file. To close my loop in my OpenSim cycling model, the only way I could do this was to apply a weld constraint between two segments of the pedal. One of the pedal segments was welded to the toe and was linked to the ground through a fixed pelvis, and the other pedal segment was connected to the fixed crank, which was also connected to the ground. In an inverse dynamics/kinematics analysis, this constraint ensures that when I rotate the crank, the other joints are forced to rotate accordingly to maintain the closed loop. However, when I run my optimal control problem, this constraint doesn't seem to be applied to the system dynamics and hence, even though I obtain a "good" solution, the two pedal segments are not welded together and so orientations of the leg and the crank are not linked. So the problem is when I call the state derivatives rather than the optimisation routine. I was hoping there might a way to ensure that the constraints are updated before computing the derivatives.

Hope that makes sense!

Adrian

[Ross Miller]

With that setup, I think the equations of motion are implemented as constraints in the optimization routine, just like any task constraints e.g. periodicity (it's all the same to IPOPT, big vector of equality constraints). If it's converging, IPOPT is essentially telling you "All the constraints I know about are satisfied with the specified threshold."

I've not modeled/simulated pedaling before, so pardon my ignorance if these are an ignorant questions:

- Why is the weld constraint needed, and why is there only one? It seems to me like (i) a bike/rider model is just a bunch of pin joints, with the pelvis and crank pinned to the ground/GCS, and (ii) there should either be zero welds (if the model does not have distinct "pedal" bodies separate from "foot" bodies) or two welds (each foot body welded to pedal body).

- How are you determining the weld constraint is being ignored?

- When you say the other joints move accordingly when you rotate the crank in the GUI, is this a one-DoF model?

If IPOPT is seemingly ignoring a constraint specified in OpenSim, my guess would be the loop is not actually closed in OpenSim, or the constraint is being accidentally over-written in some hand-written code passed to IPOPT (I do that often!).

I think I am probably not being very helpful, but maybe my questions will accidentally lead you to locate the problem =)

Ross

[Adrian Lai]

With that setup, I think the equations of motion are implemented as constraints in the optimization routine, just like any task constraints e.g. periodicity (it's all the same to IPOPT, big vector of equality constraints). If it's converging, IPOPT is essentially telling you "All the constraints I know about are satisfied with the specified threshold."

Totally agreed. I haven't had a problem till this point running. The optimisation routine works fine for other simulations with kinematic tracking.

- Why is the weld constraint needed, and why is there only one? It seems to me like (i) a bike/rider model is just a bunch of pin joints, with the pelvis and crank pinned to the ground/GCS, and (ii) there should either be zero welds (if the model does not have distinct "pedal" bodies separate from "foot" bodies) or two welds (each foot body welded to pedal body).

I tried a range of different methods but I found this as the best solution to closing a loop in OpenSim. If you use the standard OpenSim approach, the pelvis is a 6DOF joint wrt to the ground and from there, you have a lower limbs, which are sequentially parent and children to each other. However, you can only have a single joint for a single body. When you get down to the crank, you also want it to rotate wrt to the ground but this is not possible if you use the standard bottom-down approach. Hence, you require a weld constraint in your system so that the pelvis and crank can be connected to the ground. Similar approaches have been done in the past (see viewtopicPhpbb.php?f=91&t=3636&p=7954&s ... 0d1b8a9cde)

- How are you determining the weld constraint is being ignored?

I don't at the moment. I would like a way to determine this and update it. But based on my current solutions, it seems to be ignored.

- When you say the other joints move accordingly when you rotate the crank in the GUI, is this a one-DoF model?

Yes it is. I wanted to keep it as simple as possible to start off with. My goal is to investigate the effect of different crank powers on the system.

If IPOPT is seemingly ignoring a constraint specified in OpenSim, my guess would be the loop is not actually closed in OpenSim, or the constraint is being accidentally over-written in some hand-written code passed to IPOPT (I do that often!).

I'm current suspecting the former because my optimisation routine and the system dynamics are currently independent but I'll go back to my code to double check nothing is over-written.

I think I am probably not being very helpful, but maybe my questions will accidentally lead you to locate the problem =)

No no. Thanks for the discussion Ross! It's great to brainstorm possible solutions =D

Adrian

[Ross Miller]

Another thing you could try is if some of the constraints are simple enough that you can derive them by hand, you could leave them out of OpenSim and instead implement them in IPOPT. So the loop in OpenSim is (deliberately) open then you close it in IPOPT.

[Adrian Lai]

Another thing you could try is if some of the constraints are simple enough that you can derive them by hand, you could leave them out of OpenSim and instead implement them in IPOPT. So the loop in OpenSim is (deliberately) open then you close it in IPOPT.

That's what I was thinking because it's a simple weld constraint, I could find the location each body's reference frame and then set the location of the weld. Do you suggest to make it an equality or inequality constraint? I was thinking of leaving a tolerance so that the system won't be as stiff.

[Christopher Dembia]

I think solving direct collocation with musculoskeletal systems that contain kinematic constraints would be a fairly novel contribution to the field, so I'm not sure we know yet how to do this well (correct me if I am wrong, Ross).

I agree with Ross about adding path constraints, though you can get the constraint errors directly from OpenSim/Simbody, you do not need to code these by hand.

First, a bit about how Simbody works. You, as a user, choose the state of your system using SimTK::State (e.g., updY()). Then, you can cache certain quantities using realize(SimTK::Stage::Position), realize(SimTK::Stage::Acceleration), etc. Realizing to acceleration will cache udot, which you can access via SimTK::State's getUDot(). All of these cached calculations use the state values you provide, and it could be the state values you provided do not obey the multibody constraints. Realizing your system does not attempt to satisfy any constraints. Satisfying your constraints means editing the state values themselves, and realizing never alters your state values.

Realizing does, however, cache the error in the constraint equations for the given state. These constraint errors are available via SimTK::State's getQErr(), getUErr(), and getUDotErr(), for the position-level, velocity-level, and acceleration-level constraint errors.

You can use those errors as path constraints in your direct collocation problem. For holonomic constraints, it should be sufficient to use only the QErrs. I'm not sure if you also need to have the velocity-level and acceleration-level constraint errors as path constraints in your problem, but it's worth trying this as well (though the constraints will be redundant). Using the acceleration-level errors is probably a bad idea, because the accelerations (udots) being used are not those from your direct collocation approximation. Another option would be to ensure your initial state satisfies the position-level constraints, and then only enforce the acceleration-level constraints in your direct collocation problem (this is similar to how constraints are enforce during forward simulation).

See Michael Posa's work using direct collocation with constraints for robotic systems and Simbody's manuals for more information.