Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
opensim-4.0-pseudocode
from opensim import *
model = Model('mymodel.osim')
states = StateTrajectory('mystates.osimstates')
comtraj = TimeSeriesTableVec3(states.size(), 2)
comtraj.setColumnLabels(['com_position', 'com_velocity'])
for state in states:
comtraj.appendRow(state.getTime(), [
model.getMassCenter(state),
model.getMassCenterVelocity(state),
]
)
comtraj.print('com_history.sto')
comtraj.print('com_history.mat')
# OR
MATWriter(comtraj, 'com_history.mat').write()
/** Basic component that could be embedded within a Model. */
class InverseDynamics : public Component {
public:
void constructInputs() {
constructInput("udot", SimTK::Vector);
}
void constructOutputs() {
constructOutput("residual", SimTK::Vector, &solve);
}
void constructConnectors() {
constructConnector<Model>("model"); // TODO maybe unnecessary.
}
SimTK::Vector solve(const SimTK::State& s) const {
const auto& smss =
updConnector<Model>("model").getConnectee().getMatterSubsystem();
// Get appliedMobilityForces, appliedBodyForces from MultibodySystem.
Vector residualMobilityForces;
smss.calcResidualForceIgnoringConstraints(s,
appliedMobilityForces, appliedBodyForces, getInput(s, "udot"),
residualMobiityForces);
return residualMobilityForces;
}
};
/** Try to optimize memory. */
class InverseDynamics : public Component {
public:
void constructInputs() {
constructInput("udot", SimTK::Vector);
}
void constructOutputs() {
constructOutput("residual", const SimTK::Vector&,
SimTK::Stage::Velocity,
&solve);
}
void constructConnectors() {
constructConnector<Model>("model");
}
const SimTK::Vector& solve(const SimTK::State& s) const {
const auto& smss =
updConnector<Model>("model").getConnectee().getMatterSubsystem();
// Get appliedMobilityForces, appliedBodyForces from MultibodySystem.
if (!m_residualCache.isValid(s)) {
auto& residual = m_residualCache.updValue(s);
smss.calcResidualForceIgnoringConstraints(s,
appliedMobilityForces, appliedBodyForces,
getInput(s, "udot"),
residual);
m_residualCache.markAsValid(s);
}
return m_residualCache.getValue(s);
}
void extendAddToSystem(SimTK::MultibodySystem& system) const {
const_cast<InverseDynamics*>(this)->m_residualCache =
Measure_<Vector>::Result(_system->updDefaultSubsystem(),
Stage::Velocity, Stage::Acceleration);
}
private:
Measure_<Vector> m_residualCache;
};
/** Specify inputs, outputs, and connectors with macros. */
class InverseDynamics : public Component {
public:
OpenSim_DECLARE_INPUT(udot, SimTK::Vector, "Generalized accelerations. "
"Length must be number of velocity degrees of freedom.");
OpenSim_DECLARE_OUTPUT(residual, SimTK::Vector, SimTK::Stage::Velocity,
"Generalized forces necessary to satisfy Newton's 2nd law. "
"Length is number of velocity degrees of freedom.");
OpenSim_DECLARE_CONNECTOR(model, Model,
"The model to use for the inverse dynamics calculation.");
const SimTK::Vector& solve(const SimTK::State& s) const {
const auto& smss =
updConnector<Model>("model").getConnectee().getMatterSubsystem();
// Get appliedMobilityForces, appliedBodyForces from MultibodySystem.
Vector residualMobilityForces;
smss.calcResidualForceIgnoringConstraints(s,
appliedMobilityForces, appliedBodyForces, get_udot(s),
m_residualMobiityForces);
return residualMobilityForces;
}
};
/** The user provides applied forces. Should we support optional inputs, etc? */
class InverseDynamics : public Component {
public:
OpenSim_DECLARE_INPUT(udot, SimTK::Vector, "Generalized accelerations. "
"Length must be number of velocity degrees of freedom.");
OpenSim_DECLARE_INPUT(applied_mobility_forces, SimTK::Vector,
"Generalized forces to consider as applied to the model. "
"Length should be number of mobilities.");
OpenSim_DECLARE_INPUT(applied_body_forces, SimTK::Vector_<SpatialVec>,
"Body forces to consider as applied to the model. "
"Length should be number of bodies.");
OpenSim_DECLARE_OUTPUT(residual, SimTK::Vector, SimTK::Stage::Velocity,
&solve,
"Generalized forces necessary to satisfy Newton's 2nd law. "
"Length is number of velocity degrees of freedom.");
OpenSim_DECLARE_CONNECTOR(model, Model,
"The model to use for the inverse dynamics calculation.");
const SimTK::Vector& solve(const SimTK::State& s) const {
const auto& smss =
updConnector<Model>("model").getConnectee().getMatterSubsystem();
Vector residualMobilityForces;
smss.calcResidualForceIgnoringConstraints(s,
get_applied_mobility_forces(s), get_applied_body_forces(s),
get_udot(s),
residualMobiityForces);
}
};
/** Embedding InverseDynamics inside a controller. */
class FeedforwardController : public Controller {
public:
OpenSim_DECLARE_PROPERTY(kp, double, "Position gain.");
OpenSim_DECLARE_PROPERTY(kv, double, "Speed gain.");
void constructInputs() {
constructInput("q_des", SimTK::Vector);
constructInput("u_des", SimTK::Vector);
}
void constructOutput() {
constructOutput("udot_des", SimTK::Vector, SimTK::Stage::Velocity,
&desiredAcceleration);
}
SimTK::Vector desiredAcceleration(const SimTK::State& s) {
// TODO I can't use udot here?
return s.getUDot() + get_kp() * (getInput("q_des") - s.getQ()) +
get_kv() * (getInput("u_des") - s.getU());
}
void computeControls(const SimTK::State& s, SimTK::Vector& controls) {
controls = m_invdyn.getOutputValue(s, "residual");
}
void extendConnectToModel(Model& model) {
m_invdyn.updConnector<Model>("model").connect(model); // TODO
m_invdyn.updInput("udot").connect(getOutput("udot_des"));
}
private:
InverseDynamics m_invdyn;
};
```xml
<ForwardTool>
<model_filename>mymodel.osim</model_filename>
<ControllerSet name="Controllers">
<objects>
<FeedforwardController>
<kp>20</kp> <kv>100</kv>
<q_des>
<connected_to_name>/desired_joint_angles</connected_to_name>
</q_des>
<u_des>
<connected_to_name>/desired_speeds</connected_to_name>
</u_des>
</FeedforwardController>
</objects>
</ControllerSet>
<components>
<DataSource name="desired_joint_angles">
<filename>inverse_kinematics_results.sto</filename>
</DataSource>
<NumericalDifferentiator name="desired_speeds">
<method>forward</method>
<order>2</order>
<input>
<connected_to_name>/desired_joint_angles</connected_to_name>
</input>
</NumericalDifferentiatior>
</components>
</ForwardTool>
```
/** Runs the InverseDynamics component on an entire motion. Programmatic. */
class InverseDynamicsStudy : public Object {
OpenSim_DECLARE_PROPERTY("q", DataSource);
OpenSim_DECLARE_PROPERTY("residual", DataSink);
OpenSim_DECLARE_PROPERTY(model_filename, Pathname, "");
void run() const {
InverseDynamics invdyn;
// Convert labeled columns to a single vector.
// Takes a model and any Component who should have an output for each
// unconstrained coordinate. This class can handle generic error
// handling for missing data for unconstrained coordinates.
CoordinateMultiplexer mux(model, get_q());
// TODO or use a more generic multiplexer.
// Compute 2nd derivative numerically.
NumericalDifferentiator<Vector> diff;
diff.updInput("input").connect(mux.getOutput("output"));
diff.setOrder(2);
// Hook up ID's udot input to the udot produced by the differentiator.
invdyn.updInput("udot").connect(diff.getOutput("output"));
// Hook up the residual DataSink to ID's output.
// MobilizerDemultiplexer writes to a separate input for each element
// of the vector input. This requires multiconnectors.
MobilizerDemultiplexer demux(model, get_residual());
demux.updInput("input").connect(invdyn.getOutput("residual"));
// TODO creates a state trajectory to the best of its ability based on
// just joint angles.
StateTrajectory states = StateTrajectory(get_q());
//StateTrajectory states =
// StateTrajectory::fromCoordinates(get_q(), "GCVSpline");
for (const auto& state : states) {
// This causes the DataSink to be filled up.
auto residual = invdyn.solve(state);
// TODO actually, should be something like:
upd_residual().update(states); // TODO see BTK.
model.realizeReport(state); // TODO this probably makes more sense.
}
// Without having to do anything here, the DataSink has already written
// to a file. TODO should there be a "finalize" step to invoke the
// file-writing?
}
};
```
<InverseDynamicsStudy>
<model_filename>C:/data/mymodel.osim</model_filename>
<q>
<DataSource>
<filename>inverse_kinematics.sto</filename>
</DataSource>
</q>
<residual>
<DataSink>
<filename>inverse_dynamics_solution.sto</filename>
</DataSink>
</residual>
</InverseDynamicsStudy>
```
/** Quick custom InverseDynamics in Python. */
// TODO too complicated?
// Create convenience "ConstantVector" class.
```python
id = InverseDynamics()
udot = ConstantVector(Vector(1, 2, 3, 4, 5))
id.getInput("udot").connect(udot.getOutput("output"))
id.updConnector("model").connect(model) TODO non-templatized.
state = model.initSystem()
residual = id.solve(state)
```
/** Printing results to a file can be performed via a DataSink component. */
int main() {
Model model("mymodel.osim");
ForwardStudy study;
// Add components to the study.
study.add(model);
DataSink sink;
sink.setName("muscle_activity");
sink.setDataWriter(HDF5Writer("muscle_activity.h5", "/activation"));
// TODO could not write continuously to an HDF5 table; must do that once at
// the end.
// Define what the inputs to the sink are; that is, what to save.
sink.getMultiInput().append("soleus_r/activation");
// Add all muscle activation to the output; with globbing?? :)
sink.getMultiInput().append("*/activation");
study.add(sink);
// This runs the integration and fills up the DataSink.
study.run();
// Use the result without having to read a file! Very important***
const auto& sink = study.get("muscle_activity");
const TimeSeriesDataTable& dat = sink.getTable();
const auto soleus_act = dat.getColumn("soleus_r/activation");
cout << "Peak soleus activity: " << SimTK::max(soleus_act) << endl;
cout << "Average soleus activity: " << SimTK::mean(soleus_act) << endl;
sink.getTable().print();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment