def generate_results(self, root_dir, args):
model = osim.Model()
body = osim.Body("b", 1, osim.Vec3(0), osim.Inertia(0))
model.addBody(body)
joint = osim.SliderJoint("j", model.getGround(), body)
joint.updCoordinate().setName("coord")
model.addJoint(joint)
damper = osim.SpringGeneralizedForce("coord")
damper.setViscosity(-1.0)
model.addForce(damper)
actu = osim.CoordinateActuator("coord")
model.addForce(actu)
model.finalizeConnections()
moco = osim.MocoStudy()
problem = moco.updProblem()
problem.setModel(model)
problem.setTimeBounds(0, 2)
problem.setStateInfo("/jointset/j/coord/value", [-10, 10], 0, 5)
problem.setStateInfo("/jointset/j/coord/speed", [-10, 10], 0, 2)
problem.setControlInfo("/forceset/coordinateactuator", [-50, 50])
problem.addGoal(osim.MocoControlGoal("effort", 0.5))
solver = moco.initCasADiSolver()
solver.set_num_mesh_intervals(50)
solution = moco.solve()
solution.write(self.solution_file % root_dir)
def test_changing_time_bounds(self):
study = osim.MocoStudy()
problem = study.updProblem()
problem.setModel(createSlidingMassModel())
problem.setTimeBounds(0, [0, 10])
problem.setStateInfo("/slider/position/value", [0, 1], 0, 1)
problem.setStateInfo("/slider/position/speed", [-100, 100], 0, 0)
problem.setControlInfo("/actuator", [-10, 10])
problem.addGoal(osim.MocoFinalTimeGoal())
if osim.MocoTropterSolver.isAvailable():
solver = study.initTropterSolver()
solver.set_transcription_scheme("trapezoidal")
solver.set_num_mesh_intervals(19)
guess = solver.createGuess("random")
guess.setTime(osim.createVectorLinspace(20, 0.0, 3.0))
solver.setGuess(guess)
solution0 = study.solve()
problem.setTimeBounds(0, [5.8, 10])
# Editing the problem does not affect information in the Solver; the
# guess still exists.
assert (not solver.getGuess().empty())
solution = study.solve()
self.assertAlmostEqual(solution.getFinalTime(), 5.8)
def main():
import argparse
## Input parsing.
## =============
parser = argparse.ArgumentParser(
description="Generate a report given a MocoTrajectory and an associated "
"OpenSim Model. Optionally, additional reference data "
"compatible with the MocoTrajectory may be plotted "
"simultaneously.")
# Required arguments.
parser.add_argument('model_or_mocostudy',
metavar='model-or-mocostudy',
type=str,
help="OpenSim Model or MocoStudy file name "
"(including path).")
parser.add_argument('trajectory',
type=str,
help="MocoTrajectory file name (including path).")
# Optional arguments.
parser.add_argument('--bilateral',
action='store_true',
help="Plot left and right limb states and controls "
"together.")
parser.add_argument('--ref_files',
type=str,
nargs='+',
help="Paths to reference data files.")
parser.add_argument('--colormap',
type=str,
help="Matplotlib colormap from which plot colors are "
"sampled from.")
parser.add_argument('--output',
type=str,
help="Write the report to this filepath. "
"Default: the report is named "
"<trajectory-without-.sto>_report.pdf")
args = parser.parse_args()
# Load the Model and MocoTrajectory from file.
model_or_mocostudy = args.model_or_mocostudy
if model_or_mocostudy.endswith('.osim'):
model = osim.Model(model_or_mocostudy)
elif model_or_mocostudy.endswith('.omoco'):
study = osim.MocoStudy(model_or_mocostudy)
model = study.getProblem().createRep().getModelBase()
ref_files = args.ref_files
report = Report(
model=model,
trajectory_filepath=args.trajectory,
bilateral=args.bilateral,
colormap=args.colormap,
ref_files=ref_files,
output=args.output,
)
report.generate()
def solveMarkerTracking(markersRef, guess):
# Predict the optimal trajectory for a minimum time swing-up.
study = osim.MocoStudy()
study.setName("double_pendulum_track")
problem = study.updProblem()
# Model (dynamics).
problem.setModel(createDoublePendulumModel())
# Bounds.
# Arguments are name, [lower bound, upper bound],
# initial [lower bound, upper bound],
# final [lower bound, upper bound].
finalTime = markersRef.getMarkerTable().getIndependentColumn()[-1]
problem.setTimeBounds(0, finalTime)
problem.setStateInfo("/jointset/j0/q0/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j0/q0/speed", [-50, 50], 0)
problem.setStateInfo("/jointset/j1/q1/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j1/q1/speed", [-50, 50], 0)
problem.setControlInfo("/tau0", [-100, 100])
problem.setControlInfo("/tau1", [-100, 100])
# Cost: track provided marker data.
markerTracking = osim.MocoMarkerTrackingGoal()
markerTracking.setMarkersReference(markersRef)
problem.addGoal(markerTracking)
effort = osim.MocoControlGoal()
effort.setName("effort")
effort.setWeight(0.0001)
# problem.addGoal(effort)
# Configure the solver.
solver = study.initTropterSolver()
solver.set_num_mesh_intervals(50)
solver.set_verbosity(2)
solver.set_optim_solver("ipopt")
solver.set_optim_jacobian_approximation("exact")
solver.set_optim_hessian_approximation("exact")
solver.set_exact_hessian_block_sparsity_mode("dense")
solver.setGuess(guess)
# Save the problem to a setup file for reference.
study.printToXML("examplePredictAndTrack_track_markers.omoco")
# Solve the problem.
solution = study.solve()
solution.write("examplePredictAndTrack_track_markers_solution.sto")
if visualize:
study.visualize(solution)
return solution
def create_study(self):
study = osim.MocoStudy()
problem = study.updProblem()
problem.setModel(self.build_model())
problem.setStateInfo("/jointset/tx/tx/value",
[-self.width, self.width], self.xinit,
self.xfinal)
problem.setStateInfo("/jointset/ty/ty/value", [-2 * self.width, 0],
self.yinit, self.yfinal)
problem.setStateInfoPattern("/jointset/.*/speed", [-15, 15], 0, 0)
problem.setStateInfoPattern("/forceset/.*/activation", [0, 1], 0)
problem.setControlInfoPattern("/forceset/.*", [0, 1])
return study
def test_order(self):
# Can set the cost and model in any order.
study = osim.MocoStudy()
problem = study.updProblem()
problem.setTimeBounds(0, [0, 10])
problem.setStateInfo("/slider/position/value", [0, 1], 0, 1)
problem.setStateInfo("/slider/position/speed", [-100, 100], 0, 0)
problem.addGoal(osim.MocoFinalTimeGoal())
problem.setModel(createSlidingMassModel())
solver = study.initTropterSolver()
solver.set_num_mesh_intervals(19)
solver.set_transcription_scheme("trapezoidal")
finalTime = study.solve().getFinalTime()
self.assertAlmostEqual(finalTime, 2.0, places=2)
def test_default_bounds(self):
study = osim.MocoStudy()
problem = study.updProblem()
model = createSlidingMassModel()
model.finalizeFromProperties()
coord = model.updComponent("slider/position")
coord.setRangeMin(-10)
coord.setRangeMax(15)
actu = model.updComponent("actuator")
actu.setMinControl(35)
actu.setMaxControl(56)
problem.setModel(model)
phase0 = problem.getPhase(0)
# User did not specify state info explicitly.
with self.assertRaises(RuntimeError):
phase0.getStateInfo("/slider/position/value")
rep = problem.createRep()
info = rep.getStateInfo("/slider/position/value")
self.assertEqual(info.getBounds().getLower(), -10)
self.assertEqual(info.getBounds().getUpper(), 15)
# Default speed bounds.
info = rep.getStateInfo("/slider/position/speed")
self.assertEqual(info.getBounds().getLower(), -50)
self.assertEqual(info.getBounds().getUpper(), 50)
# No control info stored in the Problem.
with self.assertRaises(RuntimeError):
phase0.getControlInfo("/actuator")
# Obtained from controls.
info = rep.getControlInfo("/actuator")
self.assertEqual(info.getBounds().getLower(), 35)
self.assertEqual(info.getBounds().getUpper(), 56)
problem.setControlInfo("/actuator", [12, 15])
probinfo = phase0.getControlInfo("/actuator")
self.assertEqual(probinfo.getBounds().getLower(), 12)
self.assertEqual(probinfo.getBounds().getUpper(), 15)
rep = problem.createRep()
info = rep.getControlInfo("/actuator")
self.assertEqual(info.getBounds().getLower(), 12)
self.assertEqual(info.getBounds().getUpper(), 15)
def test_changing_costs(self):
# Changes to the costs are obeyed.
study = osim.MocoStudy()
problem = study.updProblem()
problem.setModel(createSlidingMassModel())
problem.setTimeBounds(0, [0, 10])
problem.setStateInfo("/slider/position/value", [0, 1], 0, 1)
problem.setStateInfo("/slider/position/speed", [-100, 100], 0, 0)
problem.updPhase().addGoal(osim.MocoFinalTimeGoal())
effort = osim.MocoControlGoal("effort")
problem.updPhase().addGoal(effort)
solver = study.initTropterSolver()
solver.set_transcription_scheme("trapezoidal")
finalTime0 = study.solve().getFinalTime()
# Change the weights of the costs.
effort.setWeight(0.1)
assert (study.solve().getFinalTime() < 0.8 * finalTime0)
def create_study(self, model, num_mesh_intervals=None):
moco = osim.MocoStudy()
moco.set_write_solution("results/")
solver = moco.initCasADiSolver()
solver.set_multibody_dynamics_mode('implicit')
solver.set_minimize_implicit_multibody_accelerations(True)
solver.set_implicit_multibody_accelerations_weight(0.001)
solver.set_minimize_implicit_auxiliary_derivatives(True)
solver.set_implicit_auxiliary_derivatives_weight(0.001)
if not num_mesh_intervals:
num_mesh_intervals = 50
solver.set_num_mesh_intervals(num_mesh_intervals)
solver.set_optim_convergence_tolerance(1e-3)
solver.set_optim_constraint_tolerance(1e-3)
solver.set_optim_finite_difference_scheme('forward')
# solver.set_output_interval(10)
problem = moco.updProblem()
problem.setModelCopy(model)
problem.setTimeBounds(0, [0.1, 2])
# Initial squat pose is based on
# https://simtk.org/projects/aredsimulation
# Simulation Files/ISS_ARED_Initial_States.sto; t = 1.0 seconds;
# Given that our model is planar, we adjust the squat so that the
# system's center of mass is over the feet.
# This avoid excessive tib ant activity simply to prevent the model
# from falling backwards, which is unrealistic.
squat_lumbar = np.deg2rad(-22)
problem.setStateInfo('/jointset/back/lumbar_extension/value',
[1.5 * squat_lumbar, 0.5], squat_lumbar, 0)
squat_hip = np.deg2rad(-98)
problem.setStateInfo('/jointset/hip_r/hip_extension_r/value',
[1.5 * squat_hip, 0.5], squat_hip, 0)
squat_knee = np.deg2rad(-104)
problem.setStateInfo('/jointset/knee_r/knee_extension_r/value',
[1.5 * squat_knee, 0], squat_knee, 0)
squat_ankle = np.deg2rad(-30)
problem.setStateInfo('/jointset/ankle_r/ankle_plantarflexion_r/value',
[1.5 * squat_ankle, 0.5], squat_ankle, 0)
problem.setStateInfoPattern('/jointset/.*/speed', [], 0, 0)
return moco
def test_changing_model(self):
study = osim.MocoStudy()
problem = study.updProblem()
model = createSlidingMassModel()
problem.setModel(model)
problem.setTimeBounds(0, [0, 10])
problem.setStateInfo("/slider/position/value", [0, 1], 0, 1)
problem.setStateInfo("/slider/position/speed", [-100, 100], 0, 0)
problem.addGoal(osim.MocoFinalTimeGoal())
solver = study.initTropterSolver()
solver.set_num_mesh_intervals(19)
solver.set_transcription_scheme("trapezoidal")
finalTime0 = study.solve().getFinalTime()
self.assertAlmostEqual(finalTime0, 2.00, places=2)
body = model.updComponent("body")
body.setMass(2 * body.getMass())
finalTime1 = study.solve().getFinalTime()
assert (finalTime1 > 1.1 * finalTime0)
def test_createRep(self):
model = osim.Model()
model.setName('sliding_mass')
model.set_gravity(osim.Vec3(0, 0, 0))
body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0))
model.addComponent(body)
joint = osim.SliderJoint('slider', model.getGround(), body)
coord = joint.updCoordinate()
coord.setName('position')
model.addComponent(joint)
model.finalizeConnections()
study = osim.MocoStudy()
study.setName('sliding_mass')
mp = study.updProblem()
mp.setModel(model)
pr = mp.createRep()
assert (len(pr.createStateInfoNames()) == 2)
def solvePrediction():
# Predict the optimal trajectory for a minimum time swing-up.
#
# o
# |
# o
# |
# +---o---o +
#
# iniital pose final pose
#
study = osim.MocoStudy()
study.setName("double_pendulum_predict")
problem = study.updProblem()
# Model (dynamics).
problem.setModel(createDoublePendulumModel())
# Bounds.
problem.setTimeBounds(0, [0, 5])
# Arguments are name, [lower bound, upper bound],
# initial [lower bound, upper bound],
# final [lower bound, upper bound].
problem.setStateInfo("/jointset/j0/q0/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j0/q0/speed", [-50, 50], 0, 0)
problem.setStateInfo("/jointset/j1/q1/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j1/q1/speed", [-50, 50], 0, 0)
problem.setControlInfo("/tau0", [-100, 100])
problem.setControlInfo("/tau1", [-100, 100])
# Cost: minimize final time and error from desired
# end effector position.
ftCost = osim.MocoFinalTimeGoal()
ftCost.setWeight(0.001)
problem.addGoal(ftCost)
finalCost = osim.MocoMarkerFinalGoal()
finalCost.setName("final")
finalCost.setWeight(1000.0)
finalCost.setPointName("/markerset/m1")
finalCost.setReferenceLocation(osim.Vec3(0, 2, 0))
problem.addGoal(finalCost)
# Configure the solver.
solver = study.initTropterSolver()
solver.set_num_mesh_intervals(100)
solver.set_verbosity(2)
solver.set_optim_solver("ipopt")
guess = solver.createGuess()
guess.setNumTimes(2)
guess.setTime([0, 1])
guess.setState("/jointset/j0/q0/value", [0, -math.pi])
guess.setState("/jointset/j1/q1/value", [0, 2 * math.pi])
guess.setState("/jointset/j0/q0/speed", [0, 0])
guess.setState("/jointset/j1/q1/speed", [0, 0])
guess.setControl("/tau0", [0, 0])
guess.setControl("/tau1", [0, 0])
guess.resampleWithNumTimes(10)
solver.setGuess(guess)
# Save the problem to a setup file for reference.
study.printToXML("examplePredictAndTrack_predict.omoco")
# Solve the problem.
solution = study.solve()
solution.write("examplePredictAndTrack_predict_solution.sto")
if visualize:
study.visualize(solution)
return solution
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).
problem.setStateInfo('/jointset/tx/tx/value', [-2, 2], -1.0)
problem.setStateInfo('/jointset/ty/ty/value', [-2, 2], 1.0)
solver = study.initCasADiSolver()
solution = study.solve()
animation = FuncAnimation(fig,
update,
init_func=init,
frames=range(len(data)))
pl.show()
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description="TODO")
# Required arguments.
parser.add_argument('model-or-study',
type=str,
help="OpenSim Model file name (including path).")
parser.add_argument('trajectory-prefix', type=str, help="TODO")
args = parser.parse_args()
args = vars(args)
if args['model-or-study'].endswith('.osim'):
model = osim.Model(args['model-or-study'])
elif args['model-or-study'].endswith('.omoco'):
study = osim.MocoStudy(args['model-or-study'])
model = study.getProblem().getPhase().getModelProcessor().process()
else:
raise Exception('TODO')
model.finalizeConnections()
animate_iterations(model, args['trajectory-prefix'])
joint = osim.SliderJoint('slider', model.getGround(), body)
coord = joint.updCoordinate()
coord.setName('position')
model.addComponent(joint)
# Adds the spring component
spring = osim.SpringGeneralizedForce()
spring.set_coordinate('position')
spring.setRestLength(0.)
spring.setStiffness(stiffness)
spring.setViscosity(0.)
model.addComponent(spring)
# Create MocoStudy.
# ================
moco = osim.MocoStudy()
moco.setName('oscillator_spring_stiffness')
# Define the optimal control problem.
# ===================================
problem = moco.updProblem()
# Model (dynamics).
# -----------------
problem.setModel(model)
# Bounds.
# -------
# Initial time must be 0, final time is finalTime.
problem.setTimeBounds(osim.MocoInitialBounds(0.),
osim.MocoFinalBounds(finalTime))
def test_bounds(self):
model = osim.Model()
model.setName('sliding_mass')
model.set_gravity(osim.Vec3(0, 0, 0))
body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0))
model.addComponent(body)
joint = osim.SliderJoint('slider', model.getGround(), body)
coord = joint.updCoordinate()
coord.setName('position')
model.addComponent(joint)
actu = osim.CoordinateActuator()
actu.setCoordinate(coord)
actu.setName('actuator')
actu.setOptimalForce(1)
model.addComponent(actu)
study = osim.MocoStudy()
study.setName('sliding_mass')
mp = study.updProblem()
mp.setModel(model)
ph0 = mp.getPhase()
mp.setTimeBounds(osim.MocoInitialBounds(0.),
osim.MocoFinalBounds(0.1, 5.))
assert ph0.getTimeInitialBounds().getLower() == 0
assert ph0.getTimeInitialBounds().getUpper() == 0
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 0.1)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 5.0)
mp.setTimeBounds([0.2, 0.3], [3.5])
self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 0.2)
self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 0.3)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 3.5)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 3.5)
# Use setter on MocoPhase.
ph0.setTimeBounds([2.2, 2.3], [4.5])
self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 2.2)
self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 2.3)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 4.5)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 4.5)
mp.setStateInfo('slider/position/value', osim.MocoBounds(-5, 5),
osim.MocoInitialBounds(0))
assert -5 == ph0.getStateInfo(
'slider/position/value').getBounds().getLower()
assert 5 == ph0.getStateInfo(
'slider/position/value').getBounds().getUpper()
assert isnan(
ph0.getStateInfo(
'slider/position/value').getFinalBounds().getLower())
assert isnan(
ph0.getStateInfo(
'slider/position/value').getFinalBounds().getUpper())
mp.setStateInfo('slider/position/speed', [-50, 50], [-3], 1.5)
assert -50 == ph0.getStateInfo(
'slider/position/speed').getBounds().getLower()
assert 50 == ph0.getStateInfo(
'slider/position/speed').getBounds().getUpper()
assert -3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getLower()
assert -3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getUpper()
self.assertAlmostEqual(
1.5,
ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getLower())
self.assertAlmostEqual(
1.5,
ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getUpper())
# Use setter on MocoPhase.
ph0.setStateInfo('slider/position/speed', [-6, 10], [-4, 3], [0])
assert -6 == ph0.getStateInfo(
'slider/position/speed').getBounds().getLower()
assert 10 == ph0.getStateInfo(
'slider/position/speed').getBounds().getUpper()
assert -4 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getLower()
assert 3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getUpper()
assert 0 == ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getLower()
assert 0 == ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getUpper()
# Controls.
mp.setControlInfo('actuator', osim.MocoBounds(-50, 50))
assert -50 == ph0.getControlInfo('actuator').getBounds().getLower()
assert 50 == ph0.getControlInfo('actuator').getBounds().getUpper()
mp.setControlInfo('actuator', [18])
assert 18 == ph0.getControlInfo('actuator').getBounds().getLower()
assert 18 == ph0.getControlInfo('actuator').getBounds().getUpper()
