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