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()