CoordinateActuator and Constraint Violation

[Niamh Gill]

Hi Tom,

Thanks for your help. I am trying to "realize" the methods, but am getting an error. Is it possible my syntax is wrong? I assume I should call this twice, once when I initialize the states and again after i update the states?

% code...
osimState = osimModel.initSystem() ;
osimModel.realizeAcceleration(osimState) ;

% error...
Undefined function or variable 'realizeAcceleration'.


Does computeStateVariableDerivatives not realize a constraint? Or is it that a constraint is not a "state" so to speak?


Niamh

[Thomas Uchida]

Oh, sorry- these methods were reorganized in 4.0 and don't seem to be available in 3.3 for doing what you're trying to do. To prescribe the coordinate values over time, I would suggest using an OpenSim Function rather than setting them manually at each time step from Matlab (see Coordinate::setPrescribedFunction(), https://simtk.org/api_docs/opensim/api_ ... 7fd0c5f78d). The "prescribeMotionInModel.m" script might be helpful (available from the "Scripting with Matlab" page on Confluence: http://simtk-confluence.stanford.edu:80 ... ith+Matlab).

[Niamh Gill]

The problem is using a PrescribedFunction means I will be forcing the coordinate to follow a certain trajectory. Whereas what I want to do is give the CoordinateActuator a function of angle. Which, to my understanding, is what happens with the OpenSimPlantControlsFunction in the DynamicWalker. Is there an another way of setting a PrescribedFunction based on a state instead of time?

I don't understand why if the constraint exists in the model, and the model is loaded by the function, the constraint is ignored? But, it seems the problem is that the PointConstraint isn't being recognised, therefore is there an alternative to realizeAcceleration that would recognise the constraint in 3.3? Or maybe a way on implementing the constraint each time I update the state?

[Thomas Uchida]

It looks like OpenSimPlantControlsFunction.m should do the job.

I thought you had uploaded your code, but I can't seem to find it. Are you calling initSystem() after the constraint has been added to the model? Are you setting the states manually using a method that doesn't "project onto the constraint manifold" (attempt to adjust the states so the constraints are satisfied)? Is it possible to satisfy the constraints (e.g., are you specifying the position/orientation of the pelvis as well as the foot, and are these constraints compatible)?

[Niamh Gill]

That's what I thought. But it seems that OpenSImPlantControlsFunction.m (or at least the code that I based off it - see attached) is ignoring the constraint.

I am creating the model in a separate file and using the following to add the actuator.
kneeActuator = CoordinateActuator('knee_rz') ;
kneeActuator.setName('kneeActuator') ;
kneeActuator.setOptimalForce(1) ;
kneeActuator.setMinControl(1) ;
kneeActuator.setMaxControl(2) ;
SprungKnee.addForce(kneeActuator) ;

Then the first thing I do when adding the control function is call initSystem() (see opensim_SimulateControls). I am setting the states by updating the current value, getting the derivatives, adjusting the controlVector and saving the derivatives (see opensim_actuator.m). Yes it is possible to satisfy the constraint. The model works fine if I have a torsional spring in place of the actuator or if I use a prescribedFunction (the only problem with using a prescribed function is that it controls based on time and I want control based on joint angle).

[Thomas Uchida]

Hi Niamh,

The problem is with your visualization script. In opensim_actuator.m you are (correctly) interfacing with the State by calling methods like Model::setStateVariable() and Model::computeStateVariableDerivatives(), but in OpenSimVisualizeFunction() you are calling State::updY() directly. OpenSim's States are not guaranteed to align with the state variables in the underlying Simbody system (e.g., the states may be ordered differently). If you change the color of the thigh and shank segments, you'll notice that your model's knee isn't bending in the visualizer, yet if you plot the knee angle over time you'll see that it is, indeed, changing. Try replacing the "for" loop in OpenSimVisualizeFunction() with the following:

Code: Select all

numVar = opensimModel.getNumStateVariables();
for i = 0:1:numVar-1
    CoordName = opensimModel.getStateVariableNames().get(i);
    opensimModel.setStateVariable(opensimState, char(CoordName), x(i+1));
end

Regards,
Tom

[Niamh Gill]

Hi Niamh,

The problem is with your visualization script. ... Try replacing the "for" loop in OpenSimVisualizeFunction() with the following:

Code: Select all

numVar = opensimModel.getNumStateVariables();
for i = 0:1:numVar-1
    CoordName = opensimModel.getStateVariableNames().get(i);
    opensimModel.setStateVariable(opensimState, char(CoordName), x(i+1));
end

Regards,
Tom

Hi Tom,

The above solution seems to have worked for the case where only a knee joint is included in the model.

I am now trying to introduce an actuator at the ankle joint by including an extra segment (2 new dofs). In this model (testModel.osim) there is a torsional spring at the "knee" and an actuator at the "ankle", however again the PointConstraint seems to be being ignored, eventhough I haven't changed the Control Code (opensim_simulateControls.m).

Is is possible that the spring is interfering? Or can you suggest another reason why the constraint might be being ignored?

Is there an equivalent to osimModel.getNumControls() to check if the constraints are being realized in the model?

Thanks again for your help (I've attached the relevant files),

Niamh

[Thomas Uchida]

Hi Niamh,

There seems to be a more fundamental issue here that also exists in your previous model. In opensim_actuator.m, you are filling the x_dot vector with the derivatives of the state variables and passing x_dot to Matlab's ode45, which will integrate the state without knowledge of the constraint equations (i.e., as if all coordinates were independent). Imagine what would happen if the integrator took a large step: the model could end up in a configuration with a huge constraint violation. This integration strategy would also fail to detect events. You are attempting to compensate when computeStateVariableDerivatives() is called (which, internally, projects onto the constraint manifold), but it isn't obvious that this strategy will be well behaved and the projection certainly won't result in the same solution as if the constraints were being stabilized. Also note that you're using computeStateVariableDerivatives() in opensim_actuator.m to do the projection, but there is no similar call in OpenSimVisualizeFunction(). The visualization function is setting the state exactly as it was delivered by Matlab's integrator, so the animation could differ substantially from the state that's being used to compute the derivatives. (Note that computeStateVariableDerivatives() doesn't modify the state that is passed in, so calling that method in your visualization function wouldn't make a difference.)

I think the simplest approach would be to use a model that is devoid of constraints. Rather than constraining the Foot (Contact) to the Floor with a PointConstraint, build the model from the Floor up using only mobilizers. The topology of the model would look something like this:

Floor --[BallJoint]--> Foot --[PinJoint]--> Shank --[PinJoint]--> Thigh --[WeldJoint]--> PointMass

Note that there is no FreeJoint between the Floor and the PointMass.

[Niamh Gill]

Hi Tom,

If it is a fundamental issue, does that mean it was a fluke that the problem seems to be fixed in the model with just the knee? If the issue is the constraint, is the reason this method works with the dynamic walker because it has contact elements rather that constraints? In that if I include contact elements rather than a PointConstraint, would this also fix the problem? I would like to avoid altering the topology of the model, as it doesn't make sense for what I am doing. Is there no way of realising the constraint or constraint equations within the integration? How is it that the contact elements and spring can be recognised but not the constraints? Or is it possible that the spring is being ignored as well?

For the visualisation - are you saying I should call computeStateVariables() in order to make the animation match the outputs? Does updY do the same as computStateVariables? My understanding was that updY was setting the state based on the value in x, so I thought using setStateVariable and the value in x would be the same?

% Dynamic Walker Visualisation

Code: Select all

function status = OpenSimVisualizeFunction(t, x, flag, opensimModel, opensimState, stateSize)
    status = 0;
    if(isempty(flag))
        opensimState.setTime(t(1));
        for i = 0:1:stateSize-1
            opensimState.updY().set(i, x(i+1,1));
        end
        opensimModel.getVisualizer().show(opensimState);
    end
end

% My visualisation

Code: Select all

function status = OpenSimVisualizeFunction(t, x, flag, openSimModel, openSimState)
status = 0 ;
if(isempty(flag))
    openSimState.setTime(t(1)) ;
    numVar = openSimModel.getNumStateVariables() ;
    for i = 0:1:numVar - 1
        CoordName = openSimModel.getStateVariableNames().get(i);
        openSimModel.setStateVariable(openSimState, char(CoordName), x(i + 1));
    end
    openSimModel.getVisualizer().show(openSimState) ;
end
end

[Thomas Uchida]

Hi Niamh,

If it is a fundamental issue, does that mean it was a fluke that the problem seems to be fixed in the model with just the knee?

No, the issue was still present in your first (knee-only) model. I suspect that if you compute the constraint violation, you will see that it is growing over time—though perhaps slowly. You may need to run the simulation longer for the constraint violation to be obvious in the animation.

If the issue is the constraint, is the reason this method works with the dynamic walker because it has contact elements rather that constraints? In that if I include contact elements rather than a PointConstraint, would this also fix the problem?

Yes, you could use contact elements instead to avoid any constraint-related issues, but the results will obviously be different. The "bottom-up" topology I suggested will provide (almost) identical results as you are seeking with your constrained model.

I would like to avoid altering the topology of the model, as it doesn't make sense for what I am doing.

The topology of the model won't affect the meaning of the results. After the simulation, the states from the "bottom-up" model can be trivially mapped onto your original constrained model.

Is there no way of realising the constraint or constraint equations within the integration? How is it that the contact elements and spring can be recognised but not the constraints? Or is it possible that the spring is being ignored as well?

Yes, you could implement a constraint stabilization strategy, but that would require adding ODEs to the system you're integrating in Matlab. There may also be an issue with Simbody using quaternions vs. OpenSim using Euler angles (note that state.getNY() returns 17 while model.getNumStateVariables() returns 16). You may want to check out the discussion of DAEs in the Simbody Theory Manual (https://github.com/simbody/simbody/tree ... imbody/doc) and doxygen (https://simtk.org/api_docs/simbody/3.5/ ... ml#details). Contact elements and springs apply forces to the system without reducing its degrees of freedom whereas constraints impose restrictions on relationships between the system's states.

For the visualisation - are you saying I should call computeStateVariables() in order to make the animation match the outputs?

No. The argument to Model::computeStateVariableDerivatives() is const ("const SimTK::State& s" in the doxygen), which means that although a projection occurs internally, the state you pass in is no different after the call.