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 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 np_array_to_simtk_matrix(array):
"""Convert numpy array to SimTK::Matrix"""
n, m = array.shape
M = opensim.Matrix(n, m)
for i in range(n):
for j in range(m):
M.set(i, j, array[i, j])
return M
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)
ax.set_aspect('equal')
pl.plot(tx, ty, color='black')
# Compute generalized accelerations from the solution trajectory, for use
# in computing the residual of the equations of motion.
txSpline = InterpolatedUnivariateSpline(time, tx)
# Evaluate the second derivative of the spline.
accelx = txSpline(time, 2)
tySpline = InterpolatedUnivariateSpline(time, ty)
accely = tySpline(time, 2)
# Loop through the trajectory and compute the constraint force.
model.initSystem()
statesTraj = solution.exportToStatesTrajectory(model)
matter = model.getMatterSubsystem()
constraintJacobian = osim.Matrix()
itime = 0
while itime < statesTraj.getSize():
state = statesTraj.get(itime)
model.realizePosition(state)
# Calculate the position-level constraint Jacobian, 2 x 1 (number of
# degrees of freedom by number of constraints).
# Pq = [2 * tx, -1]
matter.calcPq(state, constraintJacobian)
jac = np.array(
[constraintJacobian.get(0, 0),
constraintJacobian.get(1, 0)])
# The constraint generates generalized forces G^T * lambda.
constraintForces = jac * multiplier[itime]
# Scale down the force vector so it can be easily visualized with the
# point mass trajectory.
