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)
