simtk.org

Code: Select all

import opensim as osim
import numpy as np

model = osim.Model('./input/scaled.osim')
state = model.initSystem()
IDSolver = osim.InverseDynamicsSolver(model)

for coordinate in model.getCoordinateSet():
    coordinate.setValue(state, 1) # test in radians
    coordinate.setSpeedValue(state, 1) # test in radians/s

model.realizeVelocity(state)

# # accelerations vector must be in multibody tree order:
# CTree = model.getCoordinateNamesInMultibodyTreeOrder()
# CTree = [CTree.getElt(i) for i in range(CTree.size())]
n = state.getNU()
uDot = osim.Vector(np.ones(n)) # test in radians/s^2

IDSolver.solve(state, uDot).to_numpy()

Code: Select all

def get_torque_values(model, state, pos=None, coord_names = ["elv_angle", "shoulder_elv", "shoulder_rot", "elbow_flexion", "pro_sup", "deviation", "flexion"]):

    inverse_dynamics_solver = osim.InverseDynamicsSolver(model)
    coords = model.getCoordinateSet()
    to_keep_index = []

    for i in range(coords.getSize()):
        coord = coords.get(i)
        coord.setSpeedValue(state,0)
        if coord.getName() in coord_names:
            to_keep_index.append(i)
            if pos is not None:
                coord.setValue(state,pos[coords.get(i).getName()])
                coord.setSpeedValue(state,0)

    model.realizeVelocity(state)
    udot = osim.Vector(model.getNumSpeeds(), 0.0)
    torque_values = inverse_dynamics_solver.solve(state,udot)
    relevant_torque_values = [torque_values[to_keep_index[i]] for i in range(len(to_keep_index))]

    return relevant_torque_values

def test_torque_values(model_path, joint_names=["elv_angle", "shoulder_elv", "shoulder_rot", "elbow_flexion", "pro_sup", "deviation", "flexion"]):
    model = osim.Model(model_path)
    state = model.initSystem()

    default = get_torque_values(model,state)
    testCoord = model.getCoordinateSet().get("shoulder_elv")
    testCoord.setValue(state,3.14/2)
    raised = get_torque_values(model,state)
    # testCoord = model.getCoordinateSet().get("elbow_flexion")
    testCoord.setValue(state,3.14)
    high = get_torque_values(model,state)
    print('range max: ',testCoord.getRangeMax())

    print(default)
    print(default[1])
    print(raised[1])
    print(high[1])

test_torque_values(model_path)