def computeMarkersReference(predictedSolution):
model = createDoublePendulumModel()
model.initSystem()
states = predictedSolution.exportToStatesStorage()
# Workaround for a bug in Storage::operator=().
columnLabels = model.getStateVariableNames()
columnLabels.insert(0, "time")
states.setColumnLabels(columnLabels)
statesTraj = osim.StatesTrajectory.createFromStatesStorage(model, states)
markerTrajectories = osim.TimeSeriesTableVec3()
markerTrajectories.setColumnLabels(["/markerset/m0", "/markerset/m1"])
for state in statesTraj:
model.realizePosition(state)
m0 = model.getComponent("markerset/m0")
m1 = model.getComponent("markerset/m1")
markerTrajectories.appendRow(
state.getTime(),
osim.RowVectorVec3(
[m0.getLocationInGround(state),
m1.getLocationInGround(state)]))
# Assign a weight to each marker.
markerWeights = osim.SetMarkerWeights()
markerWeights.cloneAndAppend(osim.MarkerWeight("/markerset/m0", 1))
markerWeights.cloneAndAppend(osim.MarkerWeight("/markerset/m1", 5))
return osim.MarkersReference(markerTrajectories, markerWeights)
def _populate_references(self):
# Initialize objects needed to populate the references.
coord_functions = opensim.FunctionSet()
marker_weights = opensim.SetMarkerWeights()
# Load coordinate data, if available.
if self.coordinate_file and (self.coordinate_file != ""
and self.coordinate_file != "Unassigned"):
coordinate_values = opensim.Storage(self.coordinate_file)
# Convert to radians, if in degrees.
if not coordinate_values.isInDegrees():
self.model.getSimbodyEngine().convertDegreesToRadians(
coordinate_values)
coord_functions = opensim.GCVSplineSet(5, coordinate_values)
index = 0
for i in range(0, self.ik_task_set.getSize()):
if not self.ik_task_set.get(i).getApply():
continue
if opensim.IKCoordinateTask.safeDownCast(self.ik_task_set.get(i)):
# NOTE: Opposed to C++, a variable cannot be declared in the above if statement, so do it now.
coord_task = opensim.IKCoordinateTask.safeDownCast(
self.ik_task_set.get(i))
coord_ref = opensim.CoordinateReference()
if coord_task.getValueType(
) == opensim.IKCoordinateTask.FromFile:
if not coord_functions:
raise Exception(
"InverseKinematicsTool: value for coordinate " +
coord_task.getName() + " not found.")
index = coord_functions.getIndex(coord_task.getName(),
index)
if index >= 0:
coord_ref = opensim.CoordinateReference(
coord_task.getName(), coord_functions.get(index))
elif coord_task.getValueType(
) == opensim.IKCoordinateTask.ManualValue:
reference = opensim.Constant(
opensim.Constant(coord_task.getValue()))
coord_ref = opensim.CoordinateReference(
coord_task.getName(), reference)
else: # Assume it should be held at its default value
value = self.model.getCoordinateSet().get(
coord_task.getName()).getDefaultValue()
reference = opensim.Constant(value)
coord_ref = opensim.CoordinateReference(
coord_task.getName(), reference)
if not coord_ref:
raise Exception(
"InverseKinematicsTool: value for coordinate " +
coord_task.getName() + " not found.")
else:
coord_ref.setWeight(coord_task.getWeight())
self.coordinate_references.push_back(coord_ref)
elif opensim.IKMarkerTask.safeDownCast(self.ik_task_set.get(i)):
# NOTE: Opposed to C++, a variable cannot be declared in the above if statement, so do it now.
marker_task = opensim.IKMarkerTask.safeDownCast(
self.ik_task_set.get(i))
if marker_task.getApply():
# Only track markers that have a task and it is "applied"
marker_weights.cloneAndAppend(
opensim.MarkerWeight(marker_task.getName(),
marker_task.getWeight()))
self.markers_reference.initializeFromMarkersFile(
self.marker_file, marker_weights)