def predict(self, root_dir):
model = self.muscle_driven_model(root_dir)
moco = self.create_study(model)
problem = moco.updProblem()
problem.addGoal(osim.MocoControlGoal('effort'))
problem.addGoal(osim.MocoInitialActivationGoal('init_activation'))
problem.addGoal(osim.MocoFinalTimeGoal('time'))
solver = osim.MocoCasADiSolver.safeDownCast(moco.updSolver())
solver.resetProblem(problem)
guess = solver.createGuess()
N = guess.getNumTimes()
for muscle in model.getMuscles():
dgf = osim.DeGrooteFregly2016Muscle.safeDownCast(muscle)
if not dgf.get_ignore_tendon_compliance():
guess.setState(
'%s/normalized_tendon_force' % muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.1, 0.1))
guess.setState(
'%s/activation' % muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.05, 0.05))
guess.setControl(muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.05, 0.05))
solver.setGuess(guess)
solution = moco.solve()
solution.write(self.predict_solution_file % root_dir)
# moco.visualize(solution)
return solution
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 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_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 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 predict(self, run_time_stepping=False, exponent=2):
study = self.create_study()
problem = study.updProblem()
problem.setTimeBounds(0, [0.4, 0.8])
effort = osim.MocoControlGoal("effort")
effort.setExponent(exponent)
problem.addGoal(effort)
problem.addGoal(osim.MocoFinalTimeGoal("time", 0.01))
solution = study.solve()
# study.visualize(solution)
time_stepping = None
if run_time_stepping:
time_stepping = osim.simulateIterateWithTimeStepping(
solution, self.build_model())
return solution, time_stepping
def predict_assisted(self, root_dir):
model = self.assisted_model(root_dir)
moco = self.create_study(model)
problem = moco.updProblem()
problem.addGoal(osim.MocoControlGoal('effort'))
problem.addGoal(osim.MocoInitialActivationGoal('init_activation'))
problem.addGoal(osim.MocoFinalTimeGoal('time'))
problem.addParameter(
osim.MocoParameter('stiffness', '/forceset/spring',
'stiffness', osim.MocoBounds(0, 100)))
solver = osim.MocoCasADiSolver.safeDownCast(moco.updSolver())
solver.resetProblem(problem)
guess = solver.createGuess()
N = guess.getNumTimes()
for muscle in model.getMuscles():
dgf = osim.DeGrooteFregly2016Muscle.safeDownCast(muscle)
if not dgf.get_ignore_tendon_compliance():
guess.setState(
'%s/normalized_tendon_force' % muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.1, 0.1))
guess.setState(
'%s/activation' % muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.05, 0.05))
guess.setControl(muscle.getAbsolutePathString(),
osim.createVectorLinspace(N, 0.05, 0.05))
solver.setGuess(guess)
solver.set_parameters_require_initsystem(False)
solution = moco.solve()
# moco.visualize(solution)
solution.write(self.predict_assisted_solution_file % root_dir)
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
problem.setControlInfo("/forceset/muscle", [0.01, 1])
# Initial state constraints/costs.
if not ignore_activation_dynamics:
initial_activation = osim.MocoInitialActivationGoal()
problem.addGoal(initial_activation)
initial_activation.setName("initial_activation")
if not ignore_tendon_compliance:
initial_equilibrium = osim.MocoInitialVelocityEquilibriumDGFGoal()
problem.addGoal(initial_equilibrium)
initial_equilibrium.setName("initial_velocity_equilibrium")
# The problem converges in fewer iterations when this goal is in cost mode.
initial_equilibrium.setMode("cost")
initial_equilibrium.setWeight(0.001)
problem.addGoal(osim.MocoFinalTimeGoal())
solver = study.initCasADiSolver()
solver.set_num_mesh_intervals(25)
solver.set_multibody_dynamics_mode("implicit")
solver.set_optim_convergence_tolerance(1e-4)
solver.set_optim_constraint_tolerance(1e-4)
solution = study.solve()
osim.STOFileAdapter.write(solution.exportToStatesTable(),
"exampleHangingMuscle_states.sto")
osim.STOFileAdapter.write(solution.exportToControlsTable(),
"exampleHangingMuscle_controls.sto")
# Conduct an analysis using MuscleAnalysis and ProbeReporter.
# Create an AnalyzeTool setup file.
problem.setTimeBounds(osim.MocoInitialBounds(0.0),
osim.MocoFinalBounds(timeLB, timeUB))
# problem.setTimeBounds(0,[])
#Define marker end point goals
osimHelper.addMarkerEndPoints(taskName, problem, simModel)
#Define kinematic task bounds
osimHelper.addTaskBounds(taskName, problem, simModel, taskBoundsElv,
taskBoundsRot, taskBoundsAng)
#Add a control cost to the problem
problem.addGoal(osim.MocoControlGoal('effort', 1))
#Add a final time goal to the problem
problem.addGoal(osim.MocoFinalTimeGoal('time', 1))
#Configure the solver
solver = study.initCasADiSolver()
solver.set_num_mesh_intervals(meshInterval)
solver.set_verbosity(2)
solver.set_optim_solver('ipopt')
solver.set_optim_convergence_tolerance(1e-4)
solver.set_optim_constraint_tolerance(1e-4)
#Set the guess to an existing solution for this task from existing work
#Note that this will be slightly off given the previous study included forces
#that approximated ligaments and joint capsule passive resistance, but should
#still work as an OK start.
#Set guess in solver
