def test_SimTKArray(self):
# Initally created to test the creation of a separate simbody module.
ad = osim.SimTKArrayDouble()
ad.push_back(1)
av3 = osim.SimTKArrayVec3()
av3.push_back(osim.Vec3(8))
assert av3.at(0).get(0) == 8
def solve_with_orientations_from_file(self):
if not self.model:
try:
self.model = opensim.Model(self.model_file)
except RuntimeError:
raise RuntimeError(
"No model or valid model file was specified.")
self.model.finalizeFromProperties()
#Lock all translational coordinates
coordinates = self.model.updCoordinateSet()
for coord in coordinates:
if (coord.getMotionType() == 2
): # motion type 2 for translational type
coord.setDefaultLocked(True)
quat_table = self.experimental_orientations
quat_table.trim(self.time_start, self.time_final)
rotations = self.sensor_to_opensim_rotations # Vec3 of rotation angles
# Generate the rotation matrix
sensor_to_opensim = opensim.Rotation(opensim.SpaceRotationSequence,
rotations.get(0),
opensim.CoordinateAxis(0),
rotations.get(1),
opensim.CoordinateAxis(1),
rotations.get(2),
opensim.CoordinateAxis(2))
# Rotate the data so the Y-Axis is up
opensim.OpenSenseUtilities_rotateOrientationTable(
quat_table, sensor_to_opensim)
# Trim to the user specified time window
quat_table.trim(self.time_start, self.time_final)
orientations_data = opensim.OpenSenseUtilities_convertQuaternionsToRotations(
quat_table)
self.orientation_references = opensim.OrientationsReference(
orientations_data)
# Initialise the model
s = self.model.initSystem()
# Create the solver given the input data
ik_solver = opensim.InverseKinematicsSolver(
self.model, self.markers_reference, self.orientation_references,
self.coordinate_references)
# Set accuracy
accuracy = 1e-4 # as defined in the .cpp
ik_solver.setAccuracy(accuracy)
# Get times from the orientation data
times = self.orientation_references.getTimes()
s.setTime(times[0])
ik_solver.assemble(s)
# Create a placeholder for orientation errors, populate based on user preference according to report_errors property (in this case should always be populated)
nos = ik_solver.getNumOrientationSensorsInUse(
) # nos: number of orientation sensors
orientation_errors = opensim.SimTKArrayDouble(
nos, 0.0) # Any elements beyond nos, are allocated random numbers?
errors_per_frame = np.zeros(len(times))
errors = np.zeros((len(times), nos + 1))
fn_timer_start = time.process_time()
count = 0
for t in times:
s.setTime(t)
ik_solver.track(s)
ik_solver.computeCurrentOrientationErrors(orientation_errors)
# Store the orientation errors as a matrix
errors[count, 0] = s.getTime()
for ind in range(1, errors.shape[1]):
errors[count, ind] = orientation_errors.getElt(ind - 1)
# Sum row errors and store in a vector of overall error per frame
errors_per_frame[count] = sum(
orientation_errors.getElt(x) for x in range(nos))
count += 1
fn_timer_stop = time.process_time()
# print("Solved IMU IK for {} frames in {} s.".format(len(times), fn_timer_stop-fn_timer_start))
return errors_per_frame, errors
def solve(self):
"""Solve the inverse kinematic problem."""
# If a model was not set, try to load from file. Raise error, if neither works.
if not self.model:
try:
self.model = opensim.Model(self.model_file)
except RuntimeError:
raise RuntimeError(
"No model or valid model file was specified.")
# Check if at least one marker or coordinate was defined to be tracked.
try:
assert self.ik_task_set.getSize() > 0
except (AttributeError, AssertionError):
raise RuntimeError(
"No marker or coordinate was set to be tracked.")
# Initialize model and its default state.
self.model.finalizeFromProperties()
s = self.model.initSystem()
# Convert IK task set to references for assembly and tracking.
self._populate_references()
# Determine start and final time based on marker data, and ensure final > start.
time_markers = self.markers_reference.getValidTimeRange()
self.time_start = max(self.time_start, time_markers.get(0))
self.time_final = min(self.time_final, time_markers.get(1))
assert self.time_final >= self.time_start, "Final time {:.3f} is before start time {:.3f}.".format(
self.time_final, self.time_start)
# Get indices for established time range, number of frames, and the trial's time column.
markers_table = self.markers_reference.getMarkerTable()
start_ix = int(markers_table.getNearestRowIndexForTime(
self.time_start))
final_ix = int(markers_table.getNearestRowIndexForTime(
self.time_final))
n_frames = final_ix - start_ix + 1
times = markers_table.getIndependentColumn()
# Set up the IK solver (using OpenSim API).
ik_solver = opensim.InverseKinematicsSolver(self.model,
self.markers_reference,
self.coordinate_references,
self.constraint_weight)
ik_solver.setAccuracy(self.accuracy)
s.setTime(times[start_ix])
ik_solver.assemble(s)
# Get the number of markers the solver is actually using.
n_markers = ik_solver.getNumMarkersInUse()
# Initialize array to store squared marker errors.
marker_errors_sq = opensim.SimTKArrayDouble(n_markers, 0.0)
rmse_per_frame = np.zeros(n_frames)
# Solve IK for every frame within the time range. Time the duration.
time.clock()
for i in range(start_ix, final_ix + 1):
# Solve IK for current frame.
s.setTime(times[i])
ik_solver.track(s)
# Calculate errors and store in pre-allocated array.
ik_solver.computeCurrentSquaredMarkerErrors(marker_errors_sq)
rmse_per_frame[i - start_ix] = np.sqrt(
np.sum(marker_errors_sq.getElt(x)
for x in range(n_markers)) / n_markers)
print("Solved IK for {} frames in {} s.".format(
n_frames, time.clock()))
return rmse_per_frame