def test_SimbodyMatterSubsystem(self): model = osim.Model( os.path.join(test_dir, "gait10dof18musc_subject01.osim")) s = model.initSystem() smss = model.getMatterSubsystem() assert smss.calcSystemMass(s) == model.getTotalMass(s) assert (smss.calcSystemMassCenterLocationInGround(s)[0] == model.calcMassCenterPosition(s)[0]) assert (smss.calcSystemMassCenterLocationInGround(s)[1] == model.calcMassCenterPosition(s)[1]) assert (smss.calcSystemMassCenterLocationInGround(s)[2] == model.calcMassCenterPosition(s)[2]) J = osim.Matrix() smss.calcSystemJacobian(s, J) # 6 * number of mobilized bodies assert J.nrow() == 6 * (model.getBodySet().getSize() + 1) assert J.ncol() == model.getCoordinateSet().getSize() v = osim.Vec3() smss.calcStationJacobian(s, 2, v, J) assert J.nrow() == 3 assert J.ncol() == model.getCoordinateSet().getSize() # Inverse dynamics from SimbodyMatterSubsystem.calcResidualForce(). # For the given inputs, we will actually be computing the first column # of the mass matrix. We accomplish this by setting all inputs to 0 # except for the acceleration of the first coordinate. # f_residual = M udot + f_inertial + f_applied # = M ~[1, 0, ...] + 0 + 0 model.realizeVelocity(s) appliedMobilityForces = osim.Vector() appliedBodyForces = osim.VectorOfSpatialVec() knownUdot = osim.Vector(s.getNU(), 0.0) knownUdot[0] = 1.0 knownLambda = osim.Vector() residualMobilityForces = osim.Vector() smss.calcResidualForce(s, appliedMobilityForces, appliedBodyForces, knownUdot, knownLambda, residualMobilityForces) assert residualMobilityForces.size() == s.getNU() # Explicitly compute the first column of the mass matrix, then copmare. massMatrixFirstColumn = osim.Vector() smss.multiplyByM(s, knownUdot, massMatrixFirstColumn) assert massMatrixFirstColumn.size() == residualMobilityForces.size() for i in range(massMatrixFirstColumn.size()): self.assertAlmostEqual(massMatrixFirstColumn[i], residualMobilityForces[i]) # InverseDynamicsSolver. # Using accelerations from forward dynamics should give 0 residual. model.realizeAcceleration(s) idsolver = osim.InverseDynamicsSolver(model) residual = idsolver.solve(s, s.getUDot()) assert residual.size() == s.getNU() for i in range(residual.size()): assert abs(residual[i]) < 1e-10
def upd_model_kinematics(self, frame): # get a fresh state self.state.setTime(self.get_time(frame)) # update kinematic states self.actual_q = self.all_q[frame] self.actual_qdot = self.all_qdot[frame] self.actual_qddot = self.all_qddot[frame] self.state.setQ(osim.Vector(self.actual_q)) self.state.setU(osim.Vector(self.actual_qdot)) self.model.realizeVelocity(self.state)
def test_vector_operators(self): v = osim.Vector(5, 3) # Tests __getitem__() assert v[0] == 3 assert v[4] == 3 # Out of bounds. with self.assertRaises(RuntimeError): v[-1] with self.assertRaises(RuntimeError): v[5] # Tests __setitem__() v[0] = 15 assert v[0] == 15 with self.assertRaises(RuntimeError): v[-1] = 12 with self.assertRaises(RuntimeError): v[5] = 14 with self.assertRaises(RuntimeError): v[9] = 18 # Size. assert len(v) == 5
def test_MocoTrajectory(self): time = osim.Vector(3, 0) time.set(0, 0) time.set(1, 0.1) time.set(2, 0.2) st = osim.Matrix(3, 2) ct = osim.Matrix(3, 3) mt = osim.Matrix(3, 1) dt = osim.Matrix(3, 1) p = osim.RowVector(2, 0.0) it = osim.MocoTrajectory(time, ['s0', 's1'], ['c0', 'c1', 'c2'], ['m0'], ['d0'], ['p0', 'p1'], st, ct, mt, dt, p) it.setTime([15, 25, 35]) assert (it.getTime().get(0) == 15) assert (it.getTime().get(1) == 25) assert (it.getTime().get(2) == 35) it.setState('s0', [5, 3, 10]) s0traj = it.getState('s0') assert (s0traj[0] == 5) assert (s0traj[1] == 3) assert (s0traj[2] == 10) it.setState('s1', [2, 6, 1]) s1traj = it.getState('s1') assert (s1traj[0] == 2) assert (s1traj[1] == 6) assert (s1traj[2] == 1) it.setControl('c0', [10, 46, -5]) c0traj = it.getControl('c0') assert (c0traj[0] == 10) assert (c0traj[1] == 46) assert (c0traj[2] == -5) it.setControl('c2', [5, 12, -1]) c2traj = it.getControl('c2') assert (c2traj[0] == 5) assert (c2traj[1] == 12) assert (c2traj[2] == -1) it.setMultiplier('m0', [326, 1, 42]) m0traj = it.getMultiplier('m0') assert (m0traj[0] == 326) assert (m0traj[1] == 1) assert (m0traj[2] == 42) it.setDerivative('d0', [-10, 477, 125]) d0traj = it.getDerivative('d0') assert (d0traj[0] == -10) assert (d0traj[1] == 477) assert (d0traj[2] == 125) it.setParameter('p0', 25) it.setParameter('p1', 30) p = it.getParameters() assert (p[0] == 25) assert (p[1] == 30) p0 = it.getParameter('p0') assert (p0 == 25)
def multi_clip_reset(self, test): """ If the initialisation speed is not 1.25, never init into the standing position. If it is 1.25, mostly go into standing, sometimes RSI Do not need to learn to start at other speeds """ self.state = self.model.initializeState() self.model.equilibrateMuscles(self.state) self.istep = self.start_point = 0 self.init_traj_ix = -1 if test == False: if self.starting_speed != 1.25 or np.random.rand()>0.8: init_data = self.states_trajectories_dict[self.starting_speed] self.istep = self.start_point = np.random.randint(20,150) init_states = init_data.iloc[self.istep,1:].values vec = opensim.Vector(init_states) self.model.setStateVariableValues(self.state, vec) self.state.setTime(self.istep*self.stepsize) self.reset_manager()
def create_ground_reaction_file(self, model, solution, filepath): reaction = osim.analyzeSpatialVec(model, solution, ['/jointset/foot_ground_r\|reaction_on_child']) reaction = reaction.flatten(['_MX', '_MY', '_MZ', '_FX', '_FY', '_FZ']) prefix = '/jointset/foot_ground_r|reaction_on_child' FX = reaction.getDependentColumn(prefix + '_FX') FY = reaction.getDependentColumn(prefix + '_FY') FZ = reaction.getDependentColumn(prefix + '_FZ') MX = reaction.getDependentColumn(prefix + '_MX') MY = reaction.getDependentColumn(prefix + '_MY') MZ = reaction.getDependentColumn(prefix + '_MZ') # X coordinate of the center of pressure. x_cop = np.empty(reaction.getNumRows()) z_cop = np.empty(reaction.getNumRows()) TY = np.empty(reaction.getNumRows()) state = model.initSystem() mass = model.getTotalMass(state) gravity_accel = model.get_gravity() mass_center = model.calcMassCenterPosition(state) print(f'com = {mass_center[0], mass_center[1], mass_center[2]}') print(f'weight = {mass * abs(gravity_accel[1])}') offset = model.getBodySet().get('calcn_r').getPositionInGround(state) x_offset = offset.get(0) z_offset = offset.get(2) print(f'x_offset = {x_offset}; z_offset = {z_offset}') for i in range(reaction.getNumRows()): x = MZ[i] / FY[i] + x_offset z = -MX[i] / FY[i] + z_offset x_cop[i] = x z_cop[i] = z TY[i] = MY[i] - (x - x_offset) * FZ[i] + (z - z_offset) * FX[i] zeroMatrix = osim.Matrix(reaction.getNumRows(), 1, 0) zero = osim.Vector(reaction.getNumRows(), 0) external_loads = osim.TimeSeriesTable(reaction.getIndependentColumn(), zeroMatrix, ['zero']) external_loads.appendColumn('ground_force_vx', FX) external_loads.appendColumn('ground_force_vy', FY) external_loads.appendColumn('ground_force_vz', FZ) external_loads.appendColumn('ground_force_px', osim.Vector(x_cop)) external_loads.appendColumn('ground_force_py', zero) external_loads.appendColumn('ground_force_pz', osim.Vector(z_cop)) external_loads.appendColumn('ground_torque_x', zero) external_loads.appendColumn('ground_torque_y', osim.Vector(TY)) external_loads.appendColumn('ground_torque_z', zero) osim.STOFileAdapter.write(external_loads, filepath)
def get_open_sim_model(prob): if prob in runningOpenSimModels: return runningOpenSimModels[prob][0], runningOpenSimModels[prob][ 1], runningOpenSimModels[prob][2], runningOpenSimModels[prob][ 3], runningOpenSimModels[prob][4] else: runningOpenSimModels[prob] = [ osim.Model(subject_model), get_open_sim_model.idx ] runningOpenSimModels[prob].append( runningOpenSimModels[prob][0].initSystem()) data_storage = osim.Storage( f'{PROJECT_PATH}/{subject}/1_inverse_kinematic/{mot_file}') model = runningOpenSimModels[prob][0] model.getSimbodyEngine().convertDegreesToRadians(data_storage) data_storage.lowpassFIR(2, 6) gen_coord_function = osim.GCVSplineSet(5, data_storage) runningOpenSimModels[prob].append(list()) for frame in range(data_storage.getSize()): q = list() for idx_q in range(data_storage.getStateVector(frame).getSize()): q.append( gen_coord_function.evaluate( idx_q, 0, data_storage.getStateVector(frame).getTime())) runningOpenSimModels[prob][3].append(osim.Vector(q)) runningOpenSimModels[prob].append( np.linspace(data_storage.getFirstTime(), data_storage.getLastTime(), num=data_storage.getSize())) get_open_sim_model.idx += 1 return runningOpenSimModels[prob][0], runningOpenSimModels[prob][ 1], runningOpenSimModels[prob][2], runningOpenSimModels[prob][ 3], runningOpenSimModels[prob][4]
def test_printing(self): v1 = osim.Vec3(1, 3, 2) assert v1.__str__() == "~[1,3,2]" v2 = osim.Vector(7, 3) assert v2.__str__() == "~[3 3 3 3 3 3 3]"
# The joint's x axis must point in the global "+y" direction. jointY = osim.SliderJoint('ty', massless, osim.Vec3(0), osim.Vec3(0, 0, 0.5 * np.pi), body, osim.Vec3(0), osim.Vec3(0, 0, 0.5 * np.pi)) coordY = jointY.updCoordinate(osim.SliderJoint.Coord_TranslationX) coordY.setName('ty') model.addJoint(jointY) # Add the kinematic constraint ty = tx^2. constraint = osim.CoordinateCouplerConstraint() independentCoords = osim.ArrayStr() independentCoords.append('tx') constraint.setIndependentCoordinateNames(independentCoords) constraint.setDependentCoordinateName('ty') coefficients = osim.Vector(3, 0) # 3 elements initialized to 0. # The polynomial is c0*tx^2 + c1*tx + c2; set c0 = 1, c1 = c2 = 0. coefficients.set(0, 1) constraint.setFunction(osim.PolynomialFunction(coefficients)) model.addConstraint(constraint) study = osim.MocoStudy() problem = study.updProblem() problem.setModel(model) phase0 = problem.getPhase(0) # Setting stricter bounds for the speeds improves convergence. phase0.setDefaultSpeedBounds(osim.MocoBounds(-5, 5)) problem.setTimeBounds(0, 0.8) # Start the motion at (-1, 1).
def report_results(self, root_dir, args): solution = osim.MocoTrajectory(self.solution_file % root_dir) def residual(angle): secx = 1.0 / np.cos(angle) tanx = np.tan(angle) h = self.final_height T = self.final_time return (1.0 / np.sin(angle) - np.log( (secx + tanx) / (secx - tanx)) / (2.0 * tanx**2) - 4 * h / (self.a * T**2)) initial_angle = bisect(residual, 0.01, 0.99 * 0.5 * np.pi) tan_initial_angle = np.tan(initial_angle) c = 2 * tan_initial_angle / self.final_time def txvalue(angle): seci = 1.0 / np.cos(initial_angle) tani = np.tan(initial_angle) secx = 1.0 / np.cos(angle) tanx = np.tan(angle) return self.a / c**2 * (seci - secx - tanx * np.log( (tani + seci) / (tanx + secx))) def tyvalue(angle): seci = 1.0 / np.cos(initial_angle) tani = np.tan(initial_angle) secx = 1.0 / np.cos(angle) tanx = np.tan(angle) return self.a / (2 * c**2) * ((tani - tanx) * seci - (seci - secx) * tanx - np.log( (tani + seci) / (tanx + secx))) def txspeed(angle): seci = 1.0 / np.cos(initial_angle) secx = 1.0 / np.cos(angle) tanx = np.tan(angle) return self.a / c * np.log( (tan_initial_angle + seci) / (tanx + secx)) def tyspeed(angle): return self.a / c * (1.0 / np.cos(initial_angle) - 1.0 / np.cos(angle)) study = osim.MocoStudyFactory.createLinearTangentSteeringStudy( self.a, self.final_time, self.final_height) solver = study.initCasADiSolver() expected = solver.createGuess() txv = osim.Vector(expected.getNumTimes(), 0) tyv = osim.Vector(expected.getNumTimes(), 0) txs = osim.Vector(expected.getNumTimes(), 0) tys = osim.Vector(expected.getNumTimes(), 0) angleTraj = osim.Vector(expected.getNumTimes(), 0) for itime in range(expected.getNumTimes()): time = expected.getTime()[itime] angle = np.arctan(tan_initial_angle - c * time) angleTraj[itime] = angle txv[itime] = txvalue(angle) tyv[itime] = tyvalue(angle) txs[itime] = txspeed(angle) tys[itime] = tyspeed(angle) expected.setState('/jointset/tx/tx/value', txv) expected.setState('/jointset/ty/ty/value', tyv) expected.setState('/jointset/tx/tx/speed', txs) expected.setState('/jointset/ty/ty/speed', tys) expected.setControl('/forceset/actuator', angleTraj) rmse = expected.compareContinuousVariablesRMS(solution) print(f'root-mean-square error in solution: {rmse}') with open( os.path.join(root_dir, 'results/linear_tangent_steering_rms.txt'), 'w') as f: f.write(f'{rmse:.4f}')
def test_DataTable(self): print() table = osim.DataTable() # Set column labels. table.setColumnLabels(['0', '1', '2', '3']) assert table.getColumnLabels() == ('0', '1', '2', '3') assert table.hasColumn('0') assert table.hasColumn('2') assert not table.hasColumn('not-found') table.setColumnLabel(0, 'zero') table.setColumnLabel(2, 'two') assert table.getColumnLabel(0) == 'zero' assert table.getColumnLabel(2) == 'two' assert table.getColumnIndex('zero') == 0 assert table.getColumnIndex('two') == 2 # Append a row to the table. row = osim.RowVector([1, 2, 3, 4]) table.appendRow(0.1, row) assert table.getNumRows() == 1 assert table.getNumColumns() == 4 row0 = table.getRowAtIndex(0) assert (row0[0] == row[0] and row0[1] == row[1] and row0[2] == row[2] and row0[3] == row[3]) print(table) # Append another row to the table. row[0] *= 2 row[1] *= 2 row[2] *= 2 row[3] *= 2 table.appendRow(0.2, row) assert table.getNumRows() == 2 assert table.getNumColumns() == 4 row1 = table.getRow(0.2) assert (row1[0] == row[0] and row1[1] == row[1] and row1[2] == row[2] and row1[3] == row[3]) print(table) # Append another row to the table. row[0] *= 2 row[1] *= 2 row[2] *= 2 row[3] *= 2 table.appendRow(0.3, row) assert table.getNumRows() == 3 assert table.getNumColumns() == 4 row2 = table.getRow(0.3) assert (row2[0] == row[0] and row2[1] == row[1] and row2[2] == row[2] and row2[3] == row[3]) print(table) # Retrieve independent column. assert table.getIndependentColumn() == (0.1, 0.2, 0.3) # Retrieve dependent columns. col1 = table.getDependentColumnAtIndex(1) assert (col1[0] == 2 and col1[1] == 4 and col1[2] == 8) col3 = table.getDependentColumn('3') assert (col3[0] == 4 and col3[1] == 8 and col3[2] == 16) assert table.hasColumn(0) assert table.hasColumn(2) # Edit rows of the table. row = osim.RowVector([100, 200, 300, 400]) table.setRowAtIndex(0, row) row = table.getRowAtIndex(0) assert (row[0] == 100 and row[1] == 200 and row[2] == 300 and row[3] == 400) row0 = table.getRowAtIndex(0) row0[0] = 10 row0[1] = 10 row0[2] = 10 row0[3] = 10 row0 = table.getRowAtIndex(0) assert (row0[0] == 10 and row0[1] == 10 and row0[2] == 10 and row0[3] == 10) row2 = table.getRow(0.3) row2[0] = 20 row2[1] = 20 row2[2] = 20 row2[3] = 20 row2 = table.getRow(0.3) assert (row2[0] == 20 and row2[1] == 20 and row2[2] == 20 and row2[3] == 20) print(table) # Edit columns of the table. col1 = table.getDependentColumnAtIndex(1) col1[0] = 30 col1[1] = 30 col1[2] = 30 col1 = table.getDependentColumnAtIndex(1) assert (col1[0] == 30 and col1[1] == 30 and col1[2] == 30) col3 = table.getDependentColumn('3') col3[0] = 40 col3[1] = 40 col3[2] = 40 col3 = table.getDependentColumn('3') assert (col3[0] == 40 and col3[1] == 40 and col3[2] == 40) print(table) # Append columns to table. col = osim.Vector([1, 2, 3]) table.appendColumn("4", col) table.appendColumn("5", col) assert (table.getNumRows() == 3 and table.getNumColumns() == 6) # Add table metadata. table.addTableMetaDataString('subject-name', 'Python') table.addTableMetaDataString('subject-yob' , '1991') assert table.getTableMetaDataString('subject-name') == 'Python' assert table.getTableMetaDataString('subject-yob' ) == '1991' print(table) # Access eleemnt with index out of bounds. Exception expected. try: shouldThrow = row0[6] assert False except RuntimeError: pass try: shouldThrow = col1[5] assert False except RuntimeError: pass # Access row with index out of bounds. Exception expected. try: shouldThrow = table.getRowAtIndex(5) assert False except RuntimeError: pass # Access row with timestamp that does not exist. Exception expected. try: shouldThrow = table.getRow(5.5) assert False except RuntimeError: pass # Access column with index out of bounds. Exception expected. try: shouldThrow = table.getDependentColumnAtIndex(6) assert False except RuntimeError: pass # Access column with label that does not exist. Exception expected. try: shouldThrow = table.getDependentColumn('not-found') assert False except RuntimeError: pass # Test pack-ing of columns of DataTable. table = osim.DataTable() table.setColumnLabels(('col0_x', 'col0_y', 'col0_z', 'col1_x', 'col1_y', 'col1_z', 'col2_x', 'col2_y', 'col2_z', 'col3_x', 'col3_y', 'col3_z')) row = osim.RowVector([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) table.appendRow(1, row) row = osim.RowVector([2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]) table.appendRow(2, row) row = osim.RowVector([3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) table.appendRow(3, row) assert len(table.getColumnLabels()) == 12 assert table.getNumRows() == 3 assert table.getNumColumns() == 12 print(table) tableVec3 = table.packVec3(('_x', '_y', '_z')) tableVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3') tableVec3.getNumRows() == 3 tableVec3.getNumColumns() == 4 print(tableVec3) tableFlat = tableVec3.flatten() assert len(tableFlat.getColumnLabels()) == 12 assert tableFlat.getColumnLabel( 0) == 'col0_1' assert tableFlat.getColumnLabel(11) == 'col3_3' assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) tableVec3 = table.packVec3() tableVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3') tableVec3.getNumRows() == 3 tableVec3.getNumColumns() == 4 print(tableVec3) tableFlat = tableVec3.flatten() assert len(tableFlat.getColumnLabels()) == 12 assert tableFlat.getColumnLabel( 0) == 'col0_1' assert tableFlat.getColumnLabel(11) == 'col3_3' assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) tableUnitVec3 = table.packUnitVec3() tableUnitVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3') tableUnitVec3.getNumRows() == 3 tableUnitVec3.getNumColumns() == 4 print(tableUnitVec3) tableFlat = tableUnitVec3.flatten() assert len(tableFlat.getColumnLabels()) == 12 assert tableFlat.getColumnLabel( 0) == 'col0_1' assert tableFlat.getColumnLabel(11) == 'col3_3' assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) table.setColumnLabels(('col0.0', 'col0.1', 'col0.2', 'col0.3', 'col1.0', 'col1.1', 'col1.2', 'col1.3', 'col2.0', 'col2.1', 'col2.2', 'col2.3')) tableQuat = table.packQuaternion() tableQuat.getColumnLabels() == ('col0', 'col1', 'col2') tableQuat.getNumRows() == 3 tableQuat.getNumColumns() == 3 print(tableQuat) tableFlat = tableQuat.flatten() assert len(tableFlat.getColumnLabels()) == 12 assert tableFlat.getColumnLabel( 0) == 'col0_1' assert tableFlat.getColumnLabel(11) == 'col2_4' assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) table.setColumnLabels(('col0_0', 'col0_1', 'col0_2', 'col0_3', 'col0_4', 'col0_5', 'col1_0', 'col1_1', 'col1_2', 'col1_3', 'col1_4', 'col1_5')) tableSVec = table.packSpatialVec() tableSVec.getColumnLabels() == ('col0', 'col1') tableSVec.getNumRows() == 3 tableSVec.getNumColumns() == 2 print(tableSVec) tableFlat = tableSVec.flatten() assert len(tableFlat.getColumnLabels()) == 12 assert tableFlat.getColumnLabel( 0) == 'col0_1' assert tableFlat.getColumnLabel(11) == 'col1_6' assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat)